diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 22ada2251abc6416998c3fb43aa1b9681a5ca0d5..67402b87a4ef096bae111e5d84fe1b94b15ac193 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -171,6 +171,9 @@
propertyIsOptional="true"
propertyDefaultValue="RobotStateComponent">
<include>RobotAPI/libraries/core/remoterobot/RemoteRobot.h</include>
+ <include>RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h</include>
+ <library>RobotAPICore</library>
+ <library>RobotStatechartHelpers</library>
<method header="const VirtualRobot::RobotPtr getRobot() const">return remoteRobot;</method>
<member>VirtualRobot::RobotPtr remoteRobot;</member>
<onConnect>// initialize remote robot</onConnect>
@@ -179,13 +182,18 @@
<method header="const VirtualRobot::RobotPtr getLocalRobot() const">return localRobot;</method>
<method header="const VirtualRobot::RobotPtr getLocalCollisionRobot() const">return localCollisionRobot;</method>
+ <method header="const RobotNameHelperPtr getRobotNameHelper() const">return robotNameHelper;</method>
<member>VirtualRobot::RobotPtr localRobot;</member>
<member>VirtualRobot::RobotPtr localCollisionRobot;</member>
+ <member>RobotNameHelperPtr robotNameHelper;</member>
<onConnect>// initialize local robot</onConnect>
<onConnect>localRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponent, VirtualRobot::RobotIO::eStructure);</onConnect>
<onConnect>localCollisionRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponent, VirtualRobot::RobotIO::eCollisionModel);</onConnect>
+ <onConnect>robotNameHelper = RobotNameHelper::Create(robotStateComponent->getRobotInfo(), getSelectedStatechartProfile());</onConnect>
<stateMethod header="const VirtualRobot::RobotPtr getLocalRobot() const">return %getContext%->getLocalRobot();</stateMethod>
<stateMethod header="const VirtualRobot::RobotPtr getLocalCollisionRobot() const">return %getContext%->getLocalCollisionRobot();</stateMethod>
+ <stateMethod header="const RobotNameHelperPtr getRobotNameHelper() const">return %getContext%->getRobotNameHelper();</stateMethod>
+
</Proxy>
<Proxy include="RobotAPI/interface/components/ViewSelectionInterface.h"
humanName="Automatic View Selection"
diff --git a/data/RobotAPI/robots/Armar3/ArmarIII.xml b/data/RobotAPI/robots/Armar3/ArmarIII.xml
index f782c5bb4fcfb9c923b00a05b1356b7ba3590eb9..cf51f0c2e9aef7cc840c7caea2c2490d8526c64f 100644
--- a/data/RobotAPI/robots/Armar3/ArmarIII.xml
+++ b/data/RobotAPI/robots/Armar3/ArmarIII.xml
@@ -493,6 +493,24 @@
<Node name="Eye_Left"/>
<Node name="HeadMotionMeasurementTCP"/>
</RobotNodeSet>
-
+
+ <RobotInfo>
+ <LeftArm>
+ <KinematicChain value="LeftArm" />
+ <TorsoKinematicChain value="HipYawLeftArm" />
+ <TCP value="TCP L" />
+ <ForceTorqueSensor value="FT L" />
+ <EndEffector value="TCP L" />
+ <MemoryHandName value="???" />
+ </LeftArm>
+ <RightArm>
+ <KinematicChain value="RightArm" />
+ <TorsoKinematicChain value="HipYawRightArm" />
+ <TCP value="TCP R" />
+ <ForceTorqueSensor value="FT R" />
+ <EndEffector value="TCP R" />
+ <MemoryHandName value="???" />
+ </RightArm>
+ </RobotInfo>
</Robot>
diff --git a/scenarios/tests/statecharttestscenario/CMakeLists.txt b/scenarios/tests/statecharttestscenario/CMakeLists.txt
index e883c611031cad405e0339a12ccdeb6f1227a0fe..4698d49921e3c15027001dc3a2894b805717dfd3 100644
--- a/scenarios/tests/statecharttestscenario/CMakeLists.txt
+++ b/scenarios/tests/statecharttestscenario/CMakeLists.txt
@@ -8,7 +8,6 @@
set(SCENARIO_COMPONENTS
ConditionHandler
SystemObserver
- RobotControl
)
# optional 3rd parameter: "path/to/global/config.cfg"
diff --git a/scenarios/tests/statecharttestscenario/config/ConditionHandler.cfg b/scenarios/tests/statecharttestscenario/config/ConditionHandler.cfg
index fae5ffe0ad36b025157e2105e37c3e2a30f7506c..1350c7903c0a0b7923cc8d75e9c2568f6a7d072c 100644
--- a/scenarios/tests/statecharttestscenario/config/ConditionHandler.cfg
+++ b/scenarios/tests/statecharttestscenario/config/ConditionHandler.cfg
@@ -1,47 +1,69 @@
# ==================================================================
-# ArmarX properties
+# ConditionHandler properties
# ==================================================================
-# ArmarX.CachePath: Path for cache files
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
# Attributes:
-# - Default: ${HOME}/.armarx/mongo/.cache
+# - Default: Default value not mapped.
# - Case sensitivity: no
# - Required: no
-# ArmarX.CachePath = ${HOME}/.armarx/mongo/.cache
+# ArmarX.AdditionalPackages = Default value not mapped.
-# ArmarX.DataPath: Semicolon-separated search list for data files
+# ArmarX.ApplicationName: Application name
# Attributes:
# - Default: ""
# - Case sensitivity: no
# - Required: no
-# ArmarX.DataPath = ""
+# ArmarX.ApplicationName = ""
-# ArmarX.Verbosity: Global logging level for whole application
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
# Attributes:
-# - Default: Verbose
+# - Default: mongo/.cache
# - Case sensitivity: no
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Verbosity = Verbose
+# ArmarX.CachePath = mongo/.cache
-# ArmarX.DisableLogging: Turn logging off in whole application
+# ArmarX.ConditionHandler.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
# - Default: 0
# - Case sensitivity: no
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.DisableLogging = 0
+# ArmarX.ConditionHandler.EnableProfiling = 0
-# ArmarX.ApplicationName: Application name
+# 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.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.ConditionHandler.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ConditionHandler.ObjectName: Name of IceGrid well-known object
# Attributes:
# - Default: ""
# - Case sensitivity: no
# - Required: no
-# ArmarX.ApplicationName = ""
+# ArmarX.ConditionHandler.ObjectName = ""
+
+
+# ArmarX.ConditionHandler.Observers: Comma seperated observer list
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.ConditionHandler.Observers = ""
# ArmarX.Config: Comma-separated list of configuration files
@@ -52,41 +74,102 @@
# ArmarX.Config = ""
-# ==================================================================
-# ArmarX.ConditionHandler properties
-# ==================================================================
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.DataPath = ""
-# ArmarX.ConditionHandler.HistoryLength: Length of condition history kept by the conditionhandler
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
# Attributes:
-# - Default: 1000
+# - Default: Default value not mapped.
# - Case sensitivity: no
# - Required: no
-# ArmarX.ConditionHandler.HistoryLength = 1000
+# ArmarX.DefaultPackages = Default value not mapped.
-# ArmarX.ConditionHandler.Observers: Comma seperated observer list
+# 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: ""
+# - Default: ./config/dependencies.cfg
# - Case sensitivity: no
# - Required: no
-# ArmarX.ConditionHandler.Observers = ""
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
-# ArmarX.ConditionHandler.MinimumLoggingLevel: Local logging level only for this component
+# ArmarX.DisableLogging: Turn logging off in whole application
# Attributes:
-# - Default: Undefined
+# - Default: 0
# - Case sensitivity: no
# - Required: no
-# - Possible values: {Error, Fatal, Info, Undefined, Verbose, Warning}
-# ArmarX.ConditionHandler.MinimumLoggingLevel = Undefined
+# ArmarX.DisableLogging = 0
-# ArmarX.ConditionHandler.ObjectName: Name of IceGrid well-known object
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.EnableProfiling = 0
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: 1
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.RedirectStdout = 1
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.StartDebuggerOnCrash = 0
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
# Attributes:
# - Default: ""
# - Case sensitivity: no
# - Required: no
-# ArmarX.ConditionHandler.ObjectName = ""
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.UseTimeServer = 0
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/tests/statecharttestscenario/config/RobotControl.cfg b/scenarios/tests/statecharttestscenario/config/RobotControl.cfg
deleted file mode 100644
index cefc98c81c93ac88b47ae16bf4bf448f5b3934c8..0000000000000000000000000000000000000000
--- a/scenarios/tests/statecharttestscenario/config/RobotControl.cfg
+++ /dev/null
@@ -1,93 +0,0 @@
-# ==================================================================
-# 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.Config: Comma-separated list of configuration files
-# Attributes:
-# - Default: ""
-# - Case sensitivity: no
-# - Required: no
-# ArmarX.Config = ""
-
-
-# ==================================================================
-# ArmarX.RobotControlStateOfferer properties
-# ==================================================================
-
-# 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/statecharttestscenario/config/SystemObserver.cfg b/scenarios/tests/statecharttestscenario/config/SystemObserver.cfg
index 7102973bdf909d22014a0c940bfa2b128fffeabd..2a0fc587080ec84309c0bb0423e64269501940ca 100644
--- a/scenarios/tests/statecharttestscenario/config/SystemObserver.cfg
+++ b/scenarios/tests/statecharttestscenario/config/SystemObserver.cfg
@@ -1,13 +1,37 @@
# ==================================================================
-# ArmarX properties
+# SystemObserver properties
# ==================================================================
-# ArmarX.CachePath: Path for cache files
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
# Attributes:
-# - Default: ${HOME}/.armarx/mongo/.cache
+# - Default: Default value not mapped.
# - Case sensitivity: no
# - Required: no
-# ArmarX.CachePath = ${HOME}/.armarx/mongo/.cache
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.Config = ""
# ArmarX.DataPath: Semicolon-separated search list for data files
@@ -18,13 +42,20 @@
# ArmarX.DataPath = ""
-# ArmarX.Verbosity: Global logging level for whole application
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
# Attributes:
-# - Default: Verbose
+# - Default: Default value not mapped.
# - Case sensitivity: no
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Verbosity = Verbose
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# 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.DisableLogging: Turn logging off in whole application
@@ -32,36 +63,78 @@
# - Default: 0
# - Case sensitivity: no
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.DisableLogging = 0
-# ArmarX.ApplicationName: Application name
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
# Attributes:
-# - Default: ""
+# - Default: 0
# - Case sensitivity: no
# - Required: no
-# ArmarX.ApplicationName = ""
+# ArmarX.EnableProfiling = 0
-# ArmarX.Config: Comma-separated list of configuration files
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
-# - Default: ""
+# - Default: 1
# - Case sensitivity: no
# - Required: no
-# ArmarX.Config = ""
+# ArmarX.RedirectStdout = 1
-# ==================================================================
-# ArmarX.SystemObserver properties
-# ==================================================================
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.StartDebuggerOnCrash = 0
+
+
+# ArmarX.SystemObserver.CreateUpdateFrequenciesChannel: If true, an additional channel is created that shows the update frequency of every other channel in that observer.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.SystemObserver.CreateUpdateFrequenciesChannel = 0
+
+
+# ArmarX.SystemObserver.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.SystemObserver.EnableProfiling = 0
+
+
+# ArmarX.SystemObserver.MaxHistoryRecordFrequency: The Observer history is written with this maximum frequency. Everything faster is being skipped.
+# Attributes:
+# - Default: 50
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.SystemObserver.MaxHistoryRecordFrequency = 50
+
+
+# ArmarX.SystemObserver.MaxHistorySize: Maximum number of entries in the Observer history
+# Attributes:
+# - Default: 5000
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.SystemObserver.MaxHistorySize = 5000
+
# 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
@@ -73,4 +146,38 @@
# ArmarX.SystemObserver.ObjectName = ""
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.UseTimeServer = 0
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: no
+# - Required: no
+# ArmarX.Verbosity = Info
+
+
+
+
diff --git a/scenarios/tests/statecharttestscenario/statecharttestscenario.scx b/scenarios/tests/statecharttestscenario/statecharttestscenario.scx
index 8e36fe86ed98d45d482dd1522fa53c87f12e868c..40aeb876de507573cd16119b31e9db6e9e29766e 100644
--- a/scenarios/tests/statecharttestscenario/statecharttestscenario.scx
+++ b/scenarios/tests/statecharttestscenario/statecharttestscenario.scx
@@ -1,5 +1,6 @@
-<scenario name='statecharttestscenario' lastChange='2016-10-26.14:27:39' creation='2016-10-26.14:27:39' globalConfigName='' package='RobotAPI'>
-<application name='ConditionHandler' instance='' package='ArmarXCore'/>
-<application name='SystemObserver' instance='' package='ArmarXCore'/>
-<application name='RobotControl' instance='' package='RobotAPI'/>
-</scenario>
\ No newline at end of file
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="statecharttestscenario" creation="2016-10-26.14:27:39" globalConfigName="" package="RobotAPI">
+ <application name="ConditionHandler" instance="" package="ArmarXCore" enabled="true"/>
+ <application name="SystemObserver" instance="" package="ArmarXCore" enabled="true"/>
+</scenario>
+
diff --git a/source/RobotAPI/components/DebugDrawer/CMakeLists.txt b/source/RobotAPI/components/DebugDrawer/CMakeLists.txt
index b5462738cb54e574fa1d45aad9fa7336a73335e4..754e65a5f76875156080403b4d4f8dfd8e1513e3 100644
--- a/source/RobotAPI/components/DebugDrawer/CMakeLists.txt
+++ b/source/RobotAPI/components/DebugDrawer/CMakeLists.txt
@@ -19,7 +19,9 @@ endif()
set(COMPONENT_LIBS ArmarXCore RobotAPIInterfaces RobotAPICore ${Simox_LIBRARIES})
set(SOURCES DebugDrawerComponent.cpp)
-set(HEADERS DebugDrawerComponent.h)
+set(HEADERS DebugDrawerComponent.h
+ DebugDrawerUtils.h)
+
armarx_add_component("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.h
new file mode 100644
index 0000000000000000000000000000000000000000..b50b0a9d98870ca210f1459e4445802d06f3e922
--- /dev/null
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.h
@@ -0,0 +1,80 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <VirtualRobot/Visualization/TriMeshModel.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <Eigen/Core>
+namespace armarx
+{
+ class DebugDrawerUtils
+ {
+ public:
+ static DebugDrawerTriMesh convertTriMesh(const VirtualRobot::TriMeshModel& trimesh)
+ {
+ DebugDrawerTriMesh ddMesh;
+
+ auto addVertex = [&](const Eigen::Vector3f & v)
+ {
+ ddMesh.vertices.push_back({v(0), v(1), v(2)});
+ return (int)ddMesh.vertices.size() - 1;
+ };
+
+ auto addColor = [&](const VirtualRobot::VisualizationFactory::Color & c)
+ {
+ ddMesh.colors.push_back(DrawColor {c.r, c.g, c.b, c.transparency});
+ return (int)ddMesh.colors.size() - 1;
+ };
+ ddMesh.faces.reserve(trimesh.faces.size());
+ ddMesh.vertices.reserve(trimesh.vertices.size());
+ ddMesh.colors.reserve(trimesh.colors.size());
+
+ for (auto& f : trimesh.faces)
+ {
+ DebugDrawerFace f2;
+ f2.vertex1.vertexID = addVertex(trimesh.vertices.at(f.id1));
+ f2.vertex2.vertexID = addVertex(trimesh.vertices.at(f.id2));
+ f2.vertex3.vertexID = addVertex(trimesh.vertices.at(f.id3));
+ if (f.idNormal1 != UINT_MAX)
+ {
+ f2.vertex1.normalID = addVertex(trimesh.normals.at(f.idNormal1));
+ f2.vertex2.normalID = addVertex(trimesh.normals.at(f.idNormal2));
+ f2.vertex3.normalID = addVertex(trimesh.normals.at(f.idNormal3));
+ }
+ else
+ {
+ // f2.vertex1.normalID = f2.vertex2.normalID = f2.vertex3.normalID = addVertex(f.normal);
+ f2.normal = {f.normal.x(), f.normal.y(), f.normal.z()};
+ }
+ f2.vertex1.colorID = addColor(trimesh.colors.at(f.idColor1));
+ f2.vertex2.colorID = addColor(trimesh.colors.at(f.idColor2));
+ f2.vertex3.colorID = addColor(trimesh.colors.at(f.idColor3));
+
+ ddMesh.faces.push_back(f2);
+ }
+ return ddMesh;
+ }
+
+ };
+}
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 773e0d03b988906d5dd64344faac91ff10485b65..f9072fedc91dcb0b42e5e7f032371dcd4292e6fc 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -33,6 +33,7 @@
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
using namespace std;
using namespace VirtualRobot;
@@ -119,6 +120,8 @@ namespace armarx
}
usingTopic(robotNodeSetName + "State");
+ readRobotInfo(robotFile);
+
/*VirtualRobot::RobotNodeSetPtr pns = this->_synchronized->getRobotNodeSet("Platform");
if (pns)
@@ -134,6 +137,27 @@ namespace armarx
}*/
}
+ void RobotStateComponent::readRobotInfo(const std::string& robotFile)
+ {
+ RapidXmlReaderPtr reader = RapidXmlReader::FromFile(robotFile);
+ RapidXmlReaderNode robotNode = reader->getRoot("Robot");
+ robotInfo = readRobotInfo(robotNode.first_node("RobotInfo"));
+ }
+
+ RobotInfoNodePtr RobotStateComponent::readRobotInfo(const RapidXmlReaderNode& infoNode)
+ {
+ std::string name = infoNode.name();
+ std::string profile = infoNode.attribute_value_or_default("profile", "");
+ std::string value = infoNode.attribute_value_or_default("value", "");
+ //ARMARX_IMPORTANT << "name: " << name << "; profile: " << profile << "; value: " << value;
+ std::vector<RobotInfoNodePtr> children;
+ for(const RapidXmlReaderNode& childNode : infoNode.nodes())
+ {
+ children.push_back(readRobotInfo(childNode));
+ }
+ return new RobotInfoNode(name, profile, value, children);
+ }
+
void RobotStateComponent::onConnectComponent()
{
@@ -347,6 +371,11 @@ namespace armarx
return robotModelScaling;
}
+ RobotInfoNodePtr RobotStateComponent::getRobotInfo(const Ice::Current&) const
+ {
+ return robotInfo;
+ }
+
std::vector<string> RobotStateComponent::getArmarXPackages(const Current&) const
{
std::vector<string> result;
@@ -422,9 +451,3 @@ namespace armarx
return robotStateTopicName;
}
}
-
-
-
-
-
-
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index 95b5680f2e07aa2d22e5059e930d52da7223551b..817743c6d9c4ccc5e1ae79af641afc7a0c6df5dd 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -36,6 +36,8 @@
#include <RobotAPI/libraries/core/remoterobot/RobotStateObserver.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+
namespace armarx
{
/**
@@ -141,6 +143,9 @@ namespace armarx
bool interpolate(double time, RobotStateConfig& config) const;
float getScaling(const Ice::Current&) const override;
+
+ RobotInfoNodePtr getRobotInfo(const Ice::Current&) const override;
+
protected:
/**
* Load and create a VirtualRobot::Robot instance from the RobotFileName
@@ -167,7 +172,11 @@ namespace armarx
void reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current()) override;
void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current()) override;
void reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current()) override;
+
private:
+ void readRobotInfo(const std::string& robotFile);
+ RobotInfoNodePtr readRobotInfo(const RapidXmlReaderNode& infoNode);
+
SharedRobotInterfacePrx _synchronizedPrx;
SharedRobotServantPtr _sharedRobotServant;
VirtualRobot::RobotPtr _synchronized;
@@ -185,6 +194,7 @@ namespace armarx
float robotModelScaling;
+ RobotInfoNodePtr robotInfo;
};
}
diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.cpp b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
index d6863950dcf04be365b02cd4cee179df456678d4..a604fd00243d6dd3f06e3aa648077454fa62e4d1 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
@@ -81,6 +81,9 @@ void PlatformUnitObserver::onConnectObserver()
offerDataField("platformOdometryPose", "positionY", VariantType::Float, "Current Odometry Y position of " + platformNodeName + " in mm");
offerDataField("platformOdometryPose", "rotation", VariantType::Float, "Current Odometry Rotation of " + platformNodeName + " in radian");
+ // odometry pose is always zero in the beginning - set it so that it can be queried
+ reportPlatformOdometryPose(0, 0, 0, armarx::GlobalIceCurrent);
+
}
void PlatformUnitObserver::reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
index 543c628fedc23f4a7adb577a32c6c9841b0a6a4f..c64a2cbf103b929e127d96a1956b679b74da3320 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
@@ -2,7 +2,7 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
-
+#include <RobotAPI/components/units/RobotUnit/util/RtLogging.h>
namespace armarx
{
NJointControllerRegistration<NJointTrajectoryController> registrationControllerNJointTrajectoryController("NJointTrajectoryController");
@@ -16,8 +16,8 @@ namespace armarx
{
ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
const SensorValueBase* sv = useSensorValue(jointName);
- targets.insert(std::make_pair(jointName, ct->asA<ControlTarget1DoFActuatorVelocity>()));
- sensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorPosition>()));
+ targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
+ sensors.push_back(sv->asA<SensorValue1DoFActuatorPosition>());
}
if (cfg->jointNames.size() == 0)
{
@@ -52,6 +52,7 @@ namespace armarx
void NJointTrajectoryController::onConnectComponent()
{
plotter = getTopic<StaticPlotterInterfacePrx>("StaticPlotter");
+ dbgObs = getTopic<DebugObserverInterfacePrx>("DebugObserver");
}
@@ -60,26 +61,33 @@ namespace armarx
if (rtGetControlStruct().trajectoryCtrl)
{
TrajectoryController& contr = *rtGetControlStruct().trajectoryCtrl;
-
+ ARMARX_CHECK_EQUAL(sensors.size(), targets.size());
const auto& dimNames = contr.getTraj()->getDimensionNames();
- for (size_t i = 0; i < dimNames.size(); i++)
+ for (size_t i = 0; i < sensors.size(); i++)
+ // for(auto& s : sensors)
{
- const auto& jointName = dimNames.at(i);
- currentPos(i) = (sensors.count(jointName) == 1) ? sensors.at(jointName)->position : 0.0f;
+ ARMARX_CHECK_EQUAL(dimNames.at(i), cfg->jointNames.at(i));
+ currentPos(i) = sensors.at(i)->position;
}
auto& newVelocities = contr.update(timeSinceLastIteration.toSecondsDouble() * direction, currentPos);
+// StringVariantBaseMap positions;
+// ARMARX_RT_LOGF_INFO("Current error %.3f", contr.getCurrentError()(1)).deactivateSpam(0.1);
+
+// for (int i = 0; i < contr.getCurrentError().rows(); ++i)
+// {
+// positions[dimNames.at(i) + "-targetPosition"] = new Variant(contr.getPositions()(i));
+// positions[dimNames.at(i) + "-currentPosition"] = new Variant(currentPos(i));
+// }
+// dbgObs->setDebugChannel("positions", positions);
currentTimestamp = contr.getCurrentTimestamp();
finished = (currentTimestamp >= contr.getTraj()->rbegin()->getTimestamp() && direction == 1.0)
|| (currentTimestamp <= contr.getTraj()->begin()->getTimestamp() && direction == -1.0);
- for (size_t i = 0; i < dimNames.size(); ++i)
+ for (size_t i = 0; i < targets.size(); ++i)
{
- const auto& jointName = dimNames.at(i);
- auto it = targets.find(jointName);
- if (it != targets.end())
{
- it->second->velocity = (cfg->isPreview || finished) ? 0.0f : newVelocities(i);
+ targets.at(i)->velocity = (cfg->isPreview || finished) ? 0.0f : newVelocities(i);
}
}
@@ -168,7 +176,6 @@ namespace armarx
startTimestamp = *trajectory->getTimestamps().begin();
endTimestamp = *trajectory->getTimestamps().rbegin();
-
if (plotter)
{
StringFloatSeqDict posData;
@@ -194,7 +201,7 @@ namespace armarx
LockGuardType guard {controlDataMutex};
ARMARX_INFO << VAROUT(cfg->PID_p) << VAROUT(cfg->PID_i) << VAROUT(cfg->PID_d);
- trajectoryCtrl.reset(new TrajectoryController(trajectory, cfg->PID_p, cfg->PID_i, cfg->PID_d, false, cfg->PID_i*0.2));
+ trajectoryCtrl.reset(new TrajectoryController(trajectory, cfg->PID_p, cfg->PID_i, cfg->PID_d, false));
getWriterControlStruct().trajectoryCtrl = trajectoryCtrl;
writeControlStruct();
}
@@ -259,7 +266,7 @@ namespace armarx
{
for (auto& target : targets)
{
- target.second->velocity = 0.0f;
+ target->velocity = 0.0f;
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
index b8cab5e77bd7cb9c164e8e98cc0b6d2676b86dc7..e184ffb8f8bc3a691e842f0b7948b3752ce3257b 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
@@ -51,10 +51,9 @@ namespace armarx
double getTrajEndTime() const;
TrajectoryPtr getTrajectoryCopy() const;
private:
- // TrajectoryPtr trajectory;
IceUtil::Time startTime;
- std::map<std::string, ControlTarget1DoFActuatorVelocity*> targets;
- std::map<std::string, const SensorValue1DoFActuatorPosition*> sensors;
+ std::vector<ControlTarget1DoFActuatorVelocity*> targets;
+ std::vector<const SensorValue1DoFActuatorPosition*> sensors;
LimitlessStateSeq limitlessStates;
StaticPlotterInterfacePrx plotter;
DebugObserverInterfacePrx dbgObs;
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
index 8b0a9ac676e1913e5b56dfb961bd6957cc3e03e6..ffefaee4d9f41c1c9cc3851a5838af1479982994 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
@@ -176,6 +176,18 @@ void TrajectoryControllerSubUnit::loadJointTraj(const TrajectoryBasePtr& jointTr
auto startTime = this->jointTraj->begin()->getTimestamp();
this->jointTraj->shiftTime(-startTime);
+ bool differentJointSet = usedJoints.size() != this->jointTraj->getDimensionNames().size();
+ if (!differentJointSet)
+ {
+ for (size_t i = 0; i < usedJoints.size(); i++)
+ {
+ if (usedJoints.at(i) != this->jointTraj->getDimensionNames().at(i))
+ {
+ differentJointSet = true;
+ break;
+ }
+ }
+ }
usedJoints = this->jointTraj->getDimensionNames();
ARMARX_INFO << VAROUT(usedJoints);
@@ -185,7 +197,10 @@ void TrajectoryControllerSubUnit::loadJointTraj(const TrajectoryBasePtr& jointTr
return;
}
- jointTrajController = createTrajectoryController(usedJoints, true);
+ if (!jointTrajController || differentJointSet || recreateController)
+ {
+ jointTrajController = createTrajectoryController(usedJoints, true);
+ }
jointTrajController->setTrajectory(this->jointTraj, c);
endTime = jointTrajController->getTrajEndTime();
@@ -459,24 +474,24 @@ void TrajectoryControllerSubUnit::setup(RobotUnit* rUnit)
}
-void armarx::TrajectoryControllerSubUnit::componentPropertiesUpdated(const std::set<std::string> &changedProperties)
+void armarx::TrajectoryControllerSubUnit::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
{
ARMARX_INFO << "Changning properties";
- if(changedProperties.count("Kp"))
+ if (changedProperties.count("Kp"))
{
ARMARX_INFO << "Changning property Kp";
kp = getProperty<float>("Kp").getValue();
recreateController = true;
}
- if(changedProperties.count("Kd"))
+ if (changedProperties.count("Kd"))
{
ARMARX_INFO << "Changning property Kd";
kd = getProperty<float>("Kd").getValue();
recreateController = true;
}
- if(changedProperties.count("Ki"))
+ if (changedProperties.count("Ki"))
{
ARMARX_INFO << "Changning property Ki";
ki = getProperty<float>("Ki").getValue();
diff --git a/source/RobotAPI/interface/components/RobotNameServiceInterface.ice b/source/RobotAPI/interface/components/RobotNameServiceInterface.ice
index 6c8026c72e93223a383bbd9c3cfe8678b49f90d4..ee0d94b95bb6f23b3e99cc9bb175dab644bbdb3c 100644
--- a/source/RobotAPI/interface/components/RobotNameServiceInterface.ice
+++ b/source/RobotAPI/interface/components/RobotNameServiceInterface.ice
@@ -13,14 +13,12 @@
* 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::ArmarXObjects::TrajectoryControllerSubUnit
- * @author Stefan Reither ( stef dot reither at web dot de )
- * @date 2017
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2018
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#include <RobotAPI/interface/core/Trajectory.ice>
#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.ice>
#ifndef _ARMARX_ROBOTAPI_COMPONENTS_ROBOT_NAME_SERVICE_SLICE_
diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice
index 593e5b2b23d454fd4ceb1d6ad23b114f698c9a52..4ae1c01d6dee2d60b0c36eba031ac79a016ab947 100644
--- a/source/RobotAPI/interface/core/RobotState.ice
+++ b/source/RobotAPI/interface/core/RobotState.ice
@@ -167,6 +167,15 @@ module armarx
float getScaling();
};
+ class RobotInfoNode;
+ sequence<RobotInfoNode> RobotInfoNodeList;
+ class RobotInfoNode
+ {
+ string name;
+ string profile;
+ string value;
+ RobotInfoNodeList children;
+ };
/**
* The interface used by the RobotStateComponent which allows creating
@@ -249,14 +258,20 @@ module armarx
* @return The name of the robot node set that is represented by this component.
*
*/
- ["cpp:const"]
- idempotent string getRobotNodeSetName() throws NotInitializedException;
+ ["cpp:const"]
+ idempotent string getRobotNodeSetName() throws NotInitializedException;
+
+ ["cpp:const"]
+ idempotent RobotInfoNode getRobotInfo();
+
};
+
interface RobotStateListenerInterface
{
void reportGlobalRobotRootPose(FramedPoseBase globalPose, long timestamp, bool poseChanged);
void reportJointValues(NameValueMap jointAngles, long timestamp, bool aValueChanged);
};
+
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
index ec2e6d26660478138538502c13c997ed35c31ec8..c15f0ae31c872f1c568d04427fcf9e7a53b532e0 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
@@ -389,7 +389,7 @@ namespace armarx
// tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
// return tcpDesiredWrench;
-
+ return Eigen::Vector6f::Zero();
}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt b/source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt
index a7d4a2e9e15e6ab016dc614dd2c04b381fb5bcd6..6202780fac063d4afae5c27ddfaa183b9a8efb46 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt
@@ -18,8 +18,9 @@ if (Eigen3_FOUND AND Simox_FOUND)
endif()
set(COMPONENT_LIBS
- ArmarXCoreInterfaces RobotAPIInterfaces RobotAPICore ${Simox_LIBRARIES}
- ArmarXCore ArmarXCoreObservers
+ ArmarXCoreInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreStatechart
+ RobotAPIInterfaces RobotAPICore
+ ${Simox_LIBRARIES}
)
set(LIB_FILES
@@ -27,6 +28,8 @@ set(LIB_FILES
VelocityControllerHelper.cpp
PositionControllerHelper.cpp
ForceTorqueHelper.cpp
+KinematicUnitHelper.cpp
+RobotNameHelper.cpp
#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
)
set(LIB_HEADERS
@@ -34,6 +37,8 @@ set(LIB_HEADERS
VelocityControllerHelper.h
PositionControllerHelper.h
ForceTorqueHelper.h
+KinematicUnitHelper.h
+RobotNameHelper.h
#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
)
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e0574ba6de8812d03cc5e0a077ae911ad089c5e1
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.cpp
@@ -0,0 +1,59 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "KinematicUnitHelper.h"
+
+using namespace armarx;
+
+KinematicUnitHelper::KinematicUnitHelper(const KinematicUnitInterfacePrx& kinUnit)
+ : kinUnit(kinUnit)
+{
+}
+
+void KinematicUnitHelper::setJointAngles(const NameValueMap& jointAngles)
+{
+ kinUnit->switchControlMode(MakeControlModes(jointAngles, ePositionControl));
+ kinUnit->setJointAngles(jointAngles);
+}
+
+void KinematicUnitHelper::setJointVelocities(const NameValueMap& jointVelocities)
+{
+ kinUnit->switchControlMode(MakeControlModes(jointVelocities, eVelocityControl));
+ kinUnit->setJointVelocities(jointVelocities);
+}
+
+void KinematicUnitHelper::setJointTorques(const NameValueMap& jointTorques)
+{
+ kinUnit->switchControlMode(MakeControlModes(jointTorques, eTorqueControl));
+ kinUnit->setJointTorques(jointTorques);
+}
+
+NameControlModeMap KinematicUnitHelper::MakeControlModes(const NameValueMap& jointValues, ControlMode controlMode)
+{
+ NameControlModeMap cm;
+ for (const auto& pair : jointValues)
+ {
+ cm[pair.first] = controlMode;
+ }
+ return cm;
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..bc28651565e0289cf65fb0ffcfcb9cf354f806ec
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h
@@ -0,0 +1,48 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+
+namespace armarx
+{
+ class KinematicUnitHelper;
+ typedef boost::shared_ptr<KinematicUnitHelper> KinematicUnitHelperPtr;
+
+ class KinematicUnitHelper
+ {
+ public:
+ KinematicUnitHelper(const KinematicUnitInterfacePrx& kinUnit);
+
+ void setJointAngles(const NameValueMap& jointAngles);
+ void setJointVelocities(const NameValueMap& jointVelocities);
+ void setJointTorques(const NameValueMap& jointTorques);
+ static NameControlModeMap MakeControlModes(const NameValueMap& jointValues, ControlMode controlMode);
+
+ private:
+ KinematicUnitInterfacePrx kinUnit;
+ };
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5bf8b9eaa04605ebb19483a147b675a7e1b337d1
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -0,0 +1,195 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RobotNameHelper.h"
+#include <ArmarXCore/core/util/StringHelpers.h>
+
+using namespace armarx;
+
+const std::string RobotNameHelper::LocationLeft = "Left";
+const std::string RobotNameHelper::LocationRight = "Right";
+
+RobotNameHelper::RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile)
+ : root(Node(robotInfo))
+{
+ StatechartProfilePtr p = profile;
+ while (p && !p->isRoot())
+ {
+ profiles.emplace_back(p->getName());
+ p = p->getParent();
+ }
+ profiles.emplace_back(""); // for matching the default/root
+
+}
+
+std::string RobotNameHelper::select(const std::string& path) const
+{
+ Node node = root;
+ for (const std::string& p : Split(path, "/"))
+ {
+ node = node.select(p, profiles);
+ }
+ if (!node.isValid())
+ {
+ throw std::runtime_error("RobotNameHelper::select: path " + path + " not found");
+ }
+ return node.value();
+}
+
+RobotNameHelperPtr RobotNameHelper::Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile)
+{
+ return RobotNameHelperPtr(new RobotNameHelper(robotInfo, profile));
+}
+
+RobotNameHelper::Arm RobotNameHelper::getArm(const std::string& side)
+{
+ return Arm(shared_from_this(), side);
+}
+
+RobotNameHelper::RobotArm RobotNameHelper::getRobotArm(const std::string& side, const VirtualRobot::RobotPtr& robot)
+{
+ return RobotArm(Arm(shared_from_this(), side), robot);
+}
+
+RobotNameHelper::Node::Node(const RobotInfoNodePtr& robotInfo)
+ : robotInfo(robotInfo)
+{
+
+}
+
+std::string RobotNameHelper::Node::value()
+{
+ checkValid();
+ return robotInfo->value;
+}
+
+RobotNameHelper::Node RobotNameHelper::Node::select(const std::string& name, const std::vector<std::string>& profiles)
+{
+ if (!isValid())
+ {
+ return Node(nullptr);
+ }
+ std::map<std::string, RobotInfoNodePtr> matches;
+ for (const RobotInfoNodePtr& child : robotInfo->children)
+ {
+ if (child->name == name)
+ {
+ matches[child->profile] = child;
+ }
+ }
+ for (const std::string& p : profiles)
+ {
+ if (matches.count(p))
+ {
+ return matches.at(p);
+ }
+ }
+ return Node(nullptr);
+}
+
+bool RobotNameHelper::Node::isValid()
+{
+ return robotInfo ? true : false;
+}
+
+void RobotNameHelper::Node::checkValid()
+{
+ if (!isValid())
+ {
+ throw std::runtime_error("RobotNameHelper::Node nullptr exception");
+ }
+}
+
+
+std::string RobotNameHelper::Arm::getKinematicChain() const
+{
+ return select("KinematicChain");
+}
+
+std::string RobotNameHelper::Arm::getTorsoKinematicChain() const
+{
+ return select("TorsoKinematicChain");
+}
+
+std::string RobotNameHelper::Arm::getTCP() const
+{
+ return select("TCP");
+}
+
+std::string RobotNameHelper::Arm::getForceTorqueSensor() const
+{
+ return select("ForceTorqueSensor");
+}
+
+std::string RobotNameHelper::Arm::getEndEffector() const
+{
+ return select("EndEffector");
+}
+
+std::string RobotNameHelper::Arm::getMemoryHandName() const
+{
+ return select("MemoryHandName");
+}
+
+std::string RobotNameHelper::Arm::getHandControllerName() const
+{
+ return select("HandControllerName");
+}
+
+RobotNameHelper::RobotArm RobotNameHelper::Arm::addRobot(const VirtualRobot::RobotPtr& robot) const
+{
+ return RobotArm(*this, robot);
+}
+
+RobotNameHelper::Arm::Arm(const std::shared_ptr<RobotNameHelper>& rnh, const std::string& side)
+ : rnh(rnh), side(side)
+{
+
+}
+
+std::string RobotNameHelper::Arm::select(const std::string& path) const
+{
+ return rnh->select(side + "Arm/" + path);
+}
+
+VirtualRobot::RobotNodeSetPtr RobotNameHelper::RobotArm::getKinematicChain() const
+{
+ return robot->getRobotNodeSet(arm.getKinematicChain());
+}
+
+VirtualRobot::RobotNodeSetPtr RobotNameHelper::RobotArm::getTorsoKinematicChain() const
+{
+ return robot->getRobotNodeSet(arm.getTorsoKinematicChain());
+}
+
+VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getTCP() const
+{
+ return robot->getRobotNode(arm.getTCP());
+}
+
+RobotNameHelper::RobotArm::RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot)
+ : arm(arm), robot(robot)
+{
+
+}
+
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..0f633f431863e9c6064d559f72776487de08734d
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -0,0 +1,116 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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/>.
+ *
+ * @author Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/core/RobotState.h>
+
+#include <ArmarXCore/statechart/xmlstates/profiles/StatechartProfiles.h>
+
+#include <boost/optional.hpp>
+
+#include <VirtualRobot/Robot.h>
+
+namespace armarx
+{
+ typedef std::shared_ptr<class RobotNameHelper> RobotNameHelperPtr;
+
+ class RobotNameHelper : public std::enable_shared_from_this<RobotNameHelper>
+ {
+ public:
+ class Node
+ {
+ public:
+ Node(const RobotInfoNodePtr& robotInfo);
+ std::string value();
+
+ Node select(const std::string& name, const std::vector<std::string>& profiles);
+ bool isValid();
+ void checkValid();
+
+ private:
+ RobotInfoNodePtr robotInfo;
+ };
+
+ static const std::string LocationLeft;
+ static const std::string LocationRight;
+
+ struct RobotArm;
+ struct Arm
+ {
+ friend class RobotNameHelper;
+ public:
+
+ std::string getKinematicChain() const;
+ std::string getTorsoKinematicChain() const;
+ std::string getTCP() const;
+ std::string getForceTorqueSensor() const;
+ std::string getEndEffector() const;
+ std::string getMemoryHandName() const;
+ std::string getHandControllerName() const;
+ RobotArm addRobot(const VirtualRobot::RobotPtr& robot) const;
+
+ private:
+ Arm(const std::shared_ptr<RobotNameHelper>& rnh, const std::string& side);
+ std::string select(const std::string& path) const;
+
+ std::shared_ptr<RobotNameHelper> rnh;
+ std::string side;
+ };
+
+ struct RobotArm
+ {
+ friend class RobotNameHelper;
+ friend class Arm;
+ public:
+ VirtualRobot::RobotNodeSetPtr getKinematicChain() const;
+ VirtualRobot::RobotNodeSetPtr getTorsoKinematicChain() const;
+ VirtualRobot::RobotNodePtr getTCP() const;
+
+ private:
+ RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot);
+
+ const Arm arm;
+ VirtualRobot::RobotPtr robot;
+ };
+
+ std::string select(const std::string& path) const;
+
+ static RobotNameHelperPtr Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile);
+
+ Arm getArm(const std::string& side);
+ RobotArm getRobotArm(const std::string& side, const VirtualRobot::RobotPtr& robot);
+
+ private:
+ RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile);
+ RobotNameHelper& self()
+ {
+ return *this;
+ }
+
+ Node root;
+ std::vector<std::string> profiles;
+
+
+ };
+}
diff --git a/source/RobotAPI/libraries/core/MultiDimPIDController.h b/source/RobotAPI/libraries/core/MultiDimPIDController.h
index 8c97be3dd22267f25fc34171c961e0a74bb07907..4186cb9b15f0134e0d876194924dfe97fb7a735c 100644
--- a/source/RobotAPI/libraries/core/MultiDimPIDController.h
+++ b/source/RobotAPI/libraries/core/MultiDimPIDController.h
@@ -71,6 +71,12 @@ namespace armarx
void update(const double deltaSec, const PIDVectorX& measuredValue, const PIDVectorX& targetValue)
{
ScopedRecursiveLockPtr lock = getLock();
+ if (stackAllocations.zeroVec.rows() == 0)
+ {
+ preallocate(measuredValue.rows());
+ }
+ ARMARX_CHECK_EQUAL(measuredValue.rows(), targetValue.rows());
+ ARMARX_CHECK_EQUAL(measuredValue.rows(), stackAllocations.zeroVec.rows());
processValue = measuredValue;
target = targetValue;
@@ -96,7 +102,7 @@ namespace armarx
if (!firstRun)
{
integral += error * deltaSec;
- integral = std::min(integral,maxIntegral);
+ integral = std::min(integral, maxIntegral);
if (deltaSec > 0.0)
{
derivative = (error - previousError) / deltaSec;
diff --git a/source/RobotAPI/libraries/core/RobotPool.h b/source/RobotAPI/libraries/core/RobotPool.h
index e22c1a898bd65cd15b7876063e10a64e4d1797c9..23365e7f1edc91b175b6edf622bdd330f3044b3a 100644
--- a/source/RobotAPI/libraries/core/RobotPool.h
+++ b/source/RobotAPI/libraries/core/RobotPool.h
@@ -30,12 +30,6 @@ namespace armarx
class RobotPool
{
public:
- // struct RobotWrapper
- // {
- // ScopedLockPtr lock;
- // VirtualRobot::RobotPtr robot;
- // };
-
RobotPool(VirtualRobot::RobotPtr robot, size_t defaultSize = 1);
/**
* @brief getRobot
@@ -56,6 +50,6 @@ namespace armarx
mutable Mutex mutex;
};
typedef std::shared_ptr<RobotPool> RobotPoolPtr;
-} // namespace armarx
+}
diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index 64cd1ac6dcffd96674bcc732183fa9ca3de8b73c..e60347e7a58fb6914282dfbb659a7737cc08854c 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -26,6 +26,7 @@
#include "VectorHelpers.h"
#include <ArmarXCore/observers/AbstractObjectSerializer.h>
#include "math/MathUtils.h"
+#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
namespace armarx
{
@@ -131,9 +132,11 @@ namespace armarx
std::string Trajectory::output(const Ice::Current&) const
{
+
const ordered_view& ordv = dataMap.get<TagOrdered>();
typename ordered_view::const_iterator itMap = ordv.begin();
std::stringstream s;
+ s << "Dimensions names: " << dimensionNames;
for (; itMap != ordv.end(); itMap++)
{
s << *itMap << std::endl;
@@ -1619,21 +1622,30 @@ namespace armarx
}
destination.clear();
+
Ice::DoubleSeq timestamps = source.getTimestamps();
for (size_t dim = 0; dim < source.dim(); dim++)
{
- destination.addDimension(source.getDimensionData(dim), timestamps);
+ destination.addDimension(source.getDimensionData(dim), timestamps
+ );
}
+ CopyMetaData(source, destination);
+ }
+
+ void Trajectory::CopyMetaData(const Trajectory& source, Trajectory& destination)
+ {
destination.setDimensionNames(source.getDimensionNames());
destination.setLimitless(source.getLimitless());
}
+
void Trajectory::clear()
{
dataMap.erase(dataMap.begin(), dataMap.end());
dimensionNames.clear();
+ limitless.clear();
}
@@ -1646,12 +1658,12 @@ namespace armarx
}
Ice::DoubleSeq result;
- int size = int((endTime - startTime) / stepSize) + 1;
+ size_t size = int((endTime - startTime) / stepSize) + 1;
stepSize = (endTime - startTime) / (size - 1);
result.reserve(size);
double currentTimestamp = startTime;
- int i = 0;
+ size_t i = 0;
while (i < size)
{
@@ -1659,7 +1671,7 @@ namespace armarx
currentTimestamp += stepSize;
i++;
}
-
+ ARMARX_CHECK_EQUAL_W_HINT(result.size(), size, VAROUT(startTime) << VAROUT(endTime) << VAROUT(stepSize));
return result;
}
@@ -1715,6 +1727,71 @@ namespace armarx
return newTraj;
}
+ TrajectoryPtr Trajectory::calculateTimeOptimalTrajectory(double maxVelocity, double maxAcceleration, double maxDeviation, const IceUtil::Time& timestep)
+ {
+ Eigen::VectorXd maxVelocities = Eigen::VectorXd::Constant(dim(), maxVelocity);
+ Eigen::VectorXd maxAccelerations = Eigen::VectorXd::Constant(dim(), maxAcceleration);
+ return calculateTimeOptimalTrajectory(maxVelocities, maxAccelerations, maxDeviation, timestep);
+ }
+
+ TrajectoryPtr Trajectory::calculateTimeOptimalTrajectory(const Eigen::VectorXd& maxVelocities, const Eigen::VectorXd& maxAccelerations, double maxDeviation, IceUtil::Time const& timestep)
+ {
+
+ auto timestepValue = timestep.toSecondsDouble();
+ std::list<Eigen::VectorXd> waypointList;
+ auto dimensions = dim();
+ for (auto& waypoint : *this)
+ {
+ auto positions = waypoint.getPositions();
+ waypointList.push_back(Eigen::Map<Eigen::VectorXd>(positions.data(), dimensions));
+ }
+ VirtualRobot::TimeOptimalTrajectory timeOptimalTraj(VirtualRobot::Path(waypointList, maxDeviation),
+ maxVelocities,
+ maxAccelerations, timestepValue);
+ ARMARX_CHECK_EXPRESSION(timeOptimalTraj.isValid());
+
+
+ TrajectoryPtr newTraj = new Trajectory();
+
+ Ice::DoubleSeq newTimestamps;
+ double duration = timeOptimalTraj.getDuration();
+ newTimestamps.reserve(duration / timestepValue + 1);
+ for (double t = 0.0; t < duration; t += timestepValue)
+ {
+ newTimestamps.push_back(t);
+ }
+ newTimestamps.push_back(duration);
+
+ for (size_t d = 0; d < dimensionNames.size(); d++)
+ {
+ Ice::DoubleSeq position;
+ position.reserve(newTimestamps.size());
+ for (double t = 0.0; t < duration; t += timestepValue)
+ {
+ position.push_back(timeOptimalTraj.getPosition(t)[d]);
+ }
+ position.push_back(timeOptimalTraj.getPosition(duration)[d]);
+ newTraj->addDimension(position, newTimestamps, dimensionNames.at(d));
+
+ Ice::DoubleSeq derivs;
+ derivs.reserve(newTimestamps.size());
+
+ for (double t = 0.0; t < duration; t += timestepValue)
+ {
+ derivs.clear();
+ derivs.push_back(timeOptimalTraj.getPosition(t)[d]);
+ derivs.push_back(timeOptimalTraj.getVelocity(t)[d]);
+ newTraj->addDerivationsToDimension(d, t, derivs);
+ }
+ derivs.clear();
+ derivs.push_back(timeOptimalTraj.getPosition(duration)[d]);
+ derivs.push_back(timeOptimalTraj.getVelocity(duration)[d]);
+ newTraj->addDerivationsToDimension(d, duration, derivs);
+ }
+ newTraj->setLimitless(limitless);
+ return newTraj;
+ }
+
@@ -1890,12 +1967,18 @@ namespace armarx
{
const ordered_view& ordv = dataMap.get<TagOrdered>();
typename ordered_view::const_iterator itMap = ordv.begin();
-
+ Trajectory shiftedTraj;
+ CopyMetaData(*this, shiftedTraj);
+ auto d = dim();
for (; itMap != ordv.end(); itMap++)
{
- // since all values are shifted this operation is OK
- *const_cast<double*>(&(itMap->timestamp)) += shift;
+ for (size_t i = 0; i < d; ++i)
+ {
+ shiftedTraj.setEntries(itMap->timestamp + shift, i, *itMap->getData().at(i));
+ }
}
+ // dataMap.swap(shiftedTraj.dataMap);
+ *this = shiftedTraj;
}
void Trajectory::shiftValue(const Ice::DoubleSeq& shift)
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index f7a88f977d472ca15e996a5664c2c8f27e2d61c9..5f7613bdca048ee60f3efdcc43b53e7d9f79ca62 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -380,6 +380,8 @@ namespace armarx
static Trajectory NormalizeTimestamps(const Trajectory& traj, const double startTime = 0.0, const double endTime = 1.0);
TrajectoryPtr normalize(const double startTime = 0.0, const double endTime = 1.0);
+ TrajectoryPtr calculateTimeOptimalTrajectory(double maxVelocity, double maxAcceleration, double maxDeviation, IceUtil::Time const& timestep);
+ TrajectoryPtr calculateTimeOptimalTrajectory(const Eigen::VectorXd& maxVelocities, const Eigen::VectorXd& maxAccelerations, double maxDeviation, IceUtil::Time const& timestep);
static Ice::DoubleSeq NormalizeTimestamps(const Ice::DoubleSeq& timestamps, const double startTime = 0.0, const double endTime = 1.0);
static Ice::DoubleSeq GenerateTimestamps(double startTime = 0.0, double endTime = 1.0, double stepSize = 0.001);
template <typename T>
@@ -394,6 +396,8 @@ namespace armarx
static void CopyData(const Trajectory& source, Trajectory& destination);
+ static void CopyMetaData(const Trajectory& source, Trajectory& destination);
+
void clear();
void setPositionEntry(const double t, const size_t dimIndex, const double& y);
void setEntries(const double t, const size_t dimIndex, const Ice::DoubleSeq& y);
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index cfa6f00629a5d027130a5f0cb76bb8a488a49e09..6cd0272217df2826bb5b32f0c0b17ac3085e560e 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -489,11 +489,8 @@ namespace armarx
bool RemoteRobot::synchronizeLocalCloneToTimestamp(RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp)
{
- if (!robotStatePrx || !robot)
- {
- ARMARX_ERROR_S << "NULL data. Aborting..." << endl;
- return false;
- }
+ ARMARX_CHECK_EXPRESSION(robotStatePrx);
+ ARMARX_CHECK_EXPRESSION(robot);
RobotConfigPtr c(new RobotConfig(robot, "synchronizeLocalClone"));
RobotStateConfig config = robotStatePrx->getRobotStateAtTimestamp(timestamp);
diff --git a/source/RobotAPI/libraries/core/test/CMakeLists.txt b/source/RobotAPI/libraries/core/test/CMakeLists.txt
index a78cfe8edfe93fb1a6c2c22258f8e44c77f7a6d7..37fd064ee6553f8324c2fd2916c89dbb801f29e7 100644
--- a/source/RobotAPI/libraries/core/test/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/test/CMakeLists.txt
@@ -1,5 +1,6 @@
set(LIBS ${LIBS} RobotAPICore )
+armarx_add_test(RobotTest RobotTest.cpp "${LIBS}")
armarx_add_test(TrajectoryTest TrajectoryTest.cpp "${LIBS}")
armarx_add_test(PIDControllerTest PIDControllerTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp b/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp
index 73c9326968fc973e8e632c9de1cae7e1136a7035..3f0b6585a8bea6a195f4406799613a86b0561844 100644
--- a/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp
+++ b/source/RobotAPI/libraries/core/test/MathUtilsTest.cpp
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/test/IceTestHelper.h>
#include "../math/MathUtils.h"
#include "../math/ColorUtils.h"
+#include "../FramedPose.h"
using namespace armarx;
void check_close(float a, float b, float tolerance)
@@ -37,8 +38,21 @@ void check_close(float a, float b, float tolerance)
}
}
+BOOST_AUTO_TEST_CASE(eigenOutputTest)
+{
+ Eigen::Vector3f v;
+ v << 10, 1000, -10;
+ std::cout << "v:\n" << v << std::endl;
+
+ armarx::Vector3 v2(v);
+ std::cout << "v2:\n" << v2.output() << std::endl;
+ FramedPose p(Eigen::Matrix4f::Identity(), "", "");
+ p.position->x = 100;
+ ARMARX_INFO << "pose:\n" << p.output();
+}
+
BOOST_AUTO_TEST_CASE(fmodTest)
{
diff --git a/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp b/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
index 3eeb0ae5134a65784384716529097640b7fae53e..579c35d2261efee083ba561e3bc5e35d56528b7b 100644
--- a/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
+++ b/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
@@ -319,4 +319,17 @@ BOOST_AUTO_TEST_CASE(TrajectoryControllerUnfoldingTest)
}
+BOOST_AUTO_TEST_CASE(TrajectoryShiftTimeTest)
+{
+ FloatSeqSeq positions {{ 0, 0, 1 }, { 0, 1, 1 }};
+ TrajectoryPtr traj = new Trajectory(positions, {}, {"joint1", "joint2"});
+ ARMARX_INFO << VAROUT(traj->getDimensionNames());
+ auto startTime = traj->begin()->timestamp;
+ auto shift = 2.0;
+ traj->shiftTime(shift);
+ ARMARX_INFO << "pos: " << traj->begin()->getPosition(0);
+ BOOST_CHECK_LE(std::abs(startTime + shift - traj->begin()->timestamp), 0.0001);
+ ARMARX_INFO << VAROUT(traj->getDimensionNames());
+ BOOST_CHECK_EQUAL(traj->getDimensionNames().at(0), "joint1");
+}
diff --git a/source/RobotAPI/statecharts/CMakeLists.txt b/source/RobotAPI/statecharts/CMakeLists.txt
index 57d792ac7cef38364c95c55bdb988e6ee64a8978..6916c27001b17fd2a68aad36e7aa2acbe7e0831a 100644
--- a/source/RobotAPI/statecharts/CMakeLists.txt
+++ b/source/RobotAPI/statecharts/CMakeLists.txt
@@ -7,4 +7,5 @@ add_subdirectory(OrientedTactileSensorGroup)
add_subdirectory(TrajectoryExecutionCode)
add_subdirectory(SpeechObserverTestGroup)
-add_subdirectory(ForceTorqueUtility)
\ No newline at end of file
+add_subdirectory(ForceTorqueUtility)
+add_subdirectory(RobotNameHelperTestGroup)
\ No newline at end of file
diff --git a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/CMakeLists.txt b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..fa05832c7919bc4b44c7d4d3bf46a31d079def2c
--- /dev/null
+++ b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/CMakeLists.txt
@@ -0,0 +1,45 @@
+armarx_component_set_name("RobotNameHelperTestGroup")
+
+#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
+ ${Simox_LIBRARIES}
+ )
+
+# Sources
+
+set(SOURCES
+RobotNameHelperTestGroupRemoteStateOfferer.cpp
+)
+
+set(HEADERS
+RobotNameHelperTestGroupRemoteStateOfferer.h
+RobotNameHelperTestGroup.scgxml
+)
+
+# adds all existing state headers and sources to CMake
+armarx_generate_statechart_cmake_lists()
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
diff --git a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroup.scgxml b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroup.scgxml
new file mode 100644
index 0000000000000000000000000000000000000000..0f7629d450e81397c9aa90d70236ab153a887a2c
--- /dev/null
+++ b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroup.scgxml
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="utf-8"?>
+<StatechartGroup name="RobotNameHelperTestGroup" package="RobotAPI" generateContext="true">
+ <Proxies>
+ <Proxy value="RobotAPIInterfaces.robotStateComponent"/>
+ </Proxies>
+ <State filename="TestGetNames.xml" visibility="public"/>
+</StatechartGroup>
+
diff --git a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroupRemoteStateOfferer.cpp b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroupRemoteStateOfferer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dc941cd5a065bb5f9aecdd38f9fc4ff8bb245a24
--- /dev/null
+++ b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroupRemoteStateOfferer.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::RobotNameHelperTestGroup::RobotNameHelperTestGroupRemoteStateOfferer
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RobotNameHelperTestGroupRemoteStateOfferer.h"
+
+using namespace armarx;
+using namespace RobotNameHelperTestGroup;
+
+// DO NOT EDIT NEXT LINE
+RobotNameHelperTestGroupRemoteStateOfferer::SubClassRegistry RobotNameHelperTestGroupRemoteStateOfferer::Registry(RobotNameHelperTestGroupRemoteStateOfferer::GetName(), &RobotNameHelperTestGroupRemoteStateOfferer::CreateInstance);
+
+
+
+RobotNameHelperTestGroupRemoteStateOfferer::RobotNameHelperTestGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader) :
+ XMLRemoteStateOfferer < RobotNameHelperTestGroupStatechartContext > (reader)
+{
+}
+
+void RobotNameHelperTestGroupRemoteStateOfferer::onInitXMLRemoteStateOfferer()
+{
+
+}
+
+void RobotNameHelperTestGroupRemoteStateOfferer::onConnectXMLRemoteStateOfferer()
+{
+
+}
+
+void RobotNameHelperTestGroupRemoteStateOfferer::onExitXMLRemoteStateOfferer()
+{
+
+}
+
+// DO NOT EDIT NEXT FUNCTION
+std::string RobotNameHelperTestGroupRemoteStateOfferer::GetName()
+{
+ return "RobotNameHelperTestGroupRemoteStateOfferer";
+}
+
+// DO NOT EDIT NEXT FUNCTION
+XMLStateOffererFactoryBasePtr RobotNameHelperTestGroupRemoteStateOfferer::CreateInstance(StatechartGroupXmlReaderPtr reader)
+{
+ return XMLStateOffererFactoryBasePtr(new RobotNameHelperTestGroupRemoteStateOfferer(reader));
+}
+
+
+
diff --git a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroupRemoteStateOfferer.h b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroupRemoteStateOfferer.h
new file mode 100644
index 0000000000000000000000000000000000000000..dccee70890542c79c6dd9bf112d1de988e662fa5
--- /dev/null
+++ b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/RobotNameHelperTestGroupRemoteStateOfferer.h
@@ -0,0 +1,52 @@
+/*
+ * 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::RobotNameHelperTestGroup
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/statechart/xmlstates/XMLRemoteStateOfferer.h>
+#include "RobotNameHelperTestGroupStatechartContext.generated.h"
+
+namespace armarx
+{
+ namespace RobotNameHelperTestGroup
+ {
+ class RobotNameHelperTestGroupRemoteStateOfferer :
+ virtual public XMLRemoteStateOfferer < RobotNameHelperTestGroupStatechartContext > // Change this statechart context if you need another context (dont forget to change in the constructor as well)
+ {
+ public:
+ RobotNameHelperTestGroupRemoteStateOfferer(StatechartGroupXmlReaderPtr reader);
+
+ // inherited from RemoteStateOfferer
+ void onInitXMLRemoteStateOfferer() override;
+ void onConnectXMLRemoteStateOfferer() override;
+ void onExitXMLRemoteStateOfferer() override;
+
+ // static functions for AbstractFactory Method
+ static std::string GetName();
+ static XMLStateOffererFactoryBasePtr CreateInstance(StatechartGroupXmlReaderPtr reader);
+ static SubClassRegistry Registry;
+
+
+ };
+ }
+}
+
diff --git a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.cpp b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..50740dd9df8cab5a9eacd5fc9949695aaf3c02ee
--- /dev/null
+++ b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.cpp
@@ -0,0 +1,72 @@
+/*
+ * 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::RobotNameHelperTestGroup
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "TestGetNames.h"
+
+#include <RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h>
+
+//#include <ArmarXCore/core/time/TimeUtil.h>
+//#include <ArmarXCore/observers/variant/DatafieldRef.h>
+
+using namespace armarx;
+using namespace RobotNameHelperTestGroup;
+
+// DO NOT EDIT NEXT LINE
+TestGetNames::SubClassRegistry TestGetNames::Registry(TestGetNames::GetName(), &TestGetNames::CreateInstance);
+
+
+
+void TestGetNames::onEnter()
+{
+ // put your user code for the enter-point here
+ // execution time should be short (<100ms)
+}
+
+void TestGetNames::run()
+{
+ RobotNameHelper::RobotArm arm = getRobotNameHelper()->getArm("Left").addRobot(getRobot());
+ ARMARX_IMPORTANT << "LeftArm: " << arm.getKinematicChain()->getName();
+ ARMARX_IMPORTANT << "LeftArm TCP: " << arm.getTCP()->getName();
+ //ARMARX_IMPORTANT << "HandControllerName: " << arm.getHandControllerName();
+
+
+}
+
+//void TestGetNames::onBreak()
+//{
+// // put your user code for the breaking point here
+// // execution time should be short (<100ms)
+//}
+
+void TestGetNames::onExit()
+{
+ // put your user code for the exit point here
+ // execution time should be short (<100ms)
+}
+
+
+// DO NOT EDIT NEXT FUNCTION
+XMLStateFactoryBasePtr TestGetNames::CreateInstance(XMLStateConstructorParams stateData)
+{
+ return XMLStateFactoryBasePtr(new TestGetNames(stateData));
+}
+
diff --git a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.h b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.h
new file mode 100644
index 0000000000000000000000000000000000000000..84b0ac8e9aa28864655f75982b33f3068d0ce20e
--- /dev/null
+++ b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.h
@@ -0,0 +1,56 @@
+/*
+ * 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::RobotNameHelperTestGroup
+ * @author Simon Ottenhaus ( simon dot ottenhaus at kit dot edu )
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.generated.h>
+
+namespace armarx
+{
+ namespace RobotNameHelperTestGroup
+ {
+ class TestGetNames :
+ public TestGetNamesGeneratedBase < TestGetNames >
+ {
+ public:
+ TestGetNames(const XMLStateConstructorParams& stateData):
+ XMLStateTemplate < TestGetNames > (stateData), TestGetNamesGeneratedBase < TestGetNames > (stateData)
+ {
+ }
+
+ // inherited from StateBase
+ void onEnter() override;
+ void run() override;
+ // void onBreak() override;
+ void onExit() override;
+
+ // 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
+ };
+ }
+}
+
+
diff --git a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.xml b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.xml
new file mode 100644
index 0000000000000000000000000000000000000000..d1a1e8ab732a1257f9ba9e98b7188e5503bc2dc3
--- /dev/null
+++ b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.xml
@@ -0,0 +1,14 @@
+<?xml version="1.0" encoding="utf-8"?>
+<State version="1.2" name="TestGetNames" uuid="E97262BA-9280-43A4-844F-215F4A0614ED" width="800" height="600" type="Normal State">
+ <InputParameters/>
+ <OutputParameters/>
+ <LocalParameters/>
+ <Substates/>
+ <Events>
+ <Event name="Failure">
+ <Description>Event for statechart-internal failures or optionally user-code failures</Description>
+ </Event>
+ </Events>
+ <Transitions/>
+</State>
+