diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 75e57d387b8e4541ef29f439942728538d3feca7..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>
@@ -178,10 +181,19 @@
<stateMethod header="const VirtualRobot::RobotPtr getRobot() const">return %getContext%->getRobot();</stateMethod>
<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/DebugDrawerComponent.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
index 4746e98cf0360d1f54c1394a9ef31bcc35374869..503c8589ec95a34d0e27d2798ecf950f59d79ef8 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerComponent.cpp
@@ -1002,6 +1002,8 @@ namespace armarx
face.idNormal3 = f.vertex3.normalID;
face.idColor3 = f.vertex3.colorID;
+ face.normal = Eigen::Vector3f(f.normal.x, f.normal.y, f.normal.z);
+
triMesh->addFace(face);
}
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..ba9b07789f37f1094a6eda84517d1a8fabf381a5 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,15 @@ namespace armarx
}
usingTopic(robotNodeSetName + "State");
+ try
+ {
+ readRobotInfo(robotFile);
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Failed to read robot info from file: " << robotFile;
+ }
+
/*VirtualRobot::RobotNodeSetPtr pns = this->_synchronized->getRobotNodeSet("Platform");
if (pns)
@@ -134,6 +144,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 +378,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 +458,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/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp
index 4a9aa61ee61ebc005c2abd9e9c1550f010f7edbe..9de7dca530084c57728842fa1f213afc5a710ef1 100644
--- a/source/RobotAPI/components/units/HeadIKUnit.cpp
+++ b/source/RobotAPI/components/units/HeadIKUnit.cpp
@@ -69,7 +69,7 @@ namespace armarx
//remoteRobotPrx = robotStateComponentPrx->getSynchronizedRobot();
- localRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponentPrx, VirtualRobot::RobotIO::RobotDescription::eFull);
+ localRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponentPrx, VirtualRobot::RobotIO::RobotDescription::eStructure);
// localRobot = RemoteRobot::createLocalClone(robotStateComponentPrx);
@@ -300,17 +300,17 @@ namespace armarx
RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponentPrx);
kinematicChain = localRobot->getRobotNodeSet(robotNodeSetName);
+ auto tcpNode = kinematicChain->getTCP();
+ VirtualRobot::RobotNodePrismaticPtr virtualPrismaticJoint;
- VirtualRobot::RobotNodePrismaticPtr virtualJoint;
- for (unsigned int i = 0; i < kinematicChain->getSize(); i++)
+ virtualPrismaticJoint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(tcpNode);
+ if (!virtualPrismaticJoint)
{
- if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") == 0)
- {
- virtualJoint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodePrismatic>(kinematicChain->getNode(i));
- }
-
+ ARMARX_WARNING << deactivateSpam(10, robotNodeSetName) << "Head IK Kinematic Chain TCP is not a prismatic joint! skipping joint set";
+ continue;
}
+
// set other not-used joints to 0
for (auto& nodeName : possiblyInvolvedJointNames)
{
@@ -322,8 +322,8 @@ namespace armarx
}
}
- ARMARX_DEBUG << deactivateSpam(5) << VAROUT(virtualJoint->getName()) << " " << VAROUT(kinematicChain->getName());
- VirtualRobot::GazeIK ikSolver(kinematicChain, virtualJoint);
+ ARMARX_DEBUG << deactivateSpam(5) << VAROUT(virtualPrismaticJoint->getName()) << " " << VAROUT(kinematicChain->getName());
+ VirtualRobot::GazeIK ikSolver(kinematicChain, virtualPrismaticJoint);
ikSolver.enableJointLimitAvoidance(true);
ikSolver.setup(10, 30, 20);
//ikSolver.setVerbose(true);
@@ -373,10 +373,10 @@ namespace armarx
DrawColor {0, 1, 1, 0.2},
17);
}
-
+ auto tcpNode = kinematicChain->getTCP();
for (int i = 0; i < (signed int)kinematicChain->getSize(); i++)
{
- if (kinematicChain->getNode(i)->getName().compare("VirtualCentralGaze") != 0)
+ if (kinematicChain->getNode(i) != tcpNode)
{
targetJointAngles[kinematicChain->getNode(i)->getName()] = kinematicChain->getNode(i)->getJointValue();
controlModes[kinematicChain->getNode(i)->getName()] = ePositionControl;
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/NJointCartesianTorqueController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
index fd2e86457ef8f5bf5d15b04a99f53f4b8bb5fe3f..861a540a1d4a6060df1ada9db9ba550184ef0cc2 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
@@ -67,6 +67,10 @@ namespace armarx
};
+ /**
+ * @brief The NJointCartesianTorqueController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
class NJointCartesianTorqueController :
public NJointControllerWithTripleBuffer<NJointCartesianTorqueControllerControlData>,
public NJointCartesianTorqueControllerInterface
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
index 76529498944dfadf7752a8a7b78b27f70fba3a51..bb22fb0761e9abfcd7d899ac7c5f91f23cb42322 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
@@ -80,6 +80,10 @@ namespace armarx
};
+ /**
+ * @brief The NJointCartesianVelocityController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
class NJointCartesianVelocityController :
public NJointControllerWithTripleBuffer<NJointCartesianVelocityControllerControlData>,
public NJointCartesianVelocityControllerInterface
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
index 9a9ec7038de7169ced6e8725ce4215016e3a3d5e..ec9a18f74a1ace2886938492d36b390c9b345e3b 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
@@ -86,14 +86,13 @@ namespace armarx
void NJointCartesianVelocityControllerWithRamp::rtPreActivateController()
{
- ARMARX_IMPORTANT << "rtPreActivateController start";
Eigen::VectorXf currentJointVelocities(velocitySensors.size());
for (size_t i = 0; i < velocitySensors.size(); i++)
{
currentJointVelocities(i) = *velocitySensors.at(i);
}
controller->setCurrentJointVelocity(currentJointVelocities);
- ARMARX_IMPORTANT << "rtPreActivateController end";
+ ARMARX_IMPORTANT << "initial joint velocities: " << currentJointVelocities.transpose();
}
void NJointCartesianVelocityControllerWithRamp::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
index a32135d756cb90279724cdd821039f32e71b5552..eecac37cfdc1e96f2b16d2f4666a432b907764ee 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
@@ -88,6 +88,10 @@ namespace armarx
+ /**
+ * @brief The NJointCartesianVelocityControllerWithRamp class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
class NJointCartesianVelocityControllerWithRamp :
public NJointControllerWithTripleBuffer<NJointCartesianVelocityControllerWithRampControlData>,
public NJointCartesianVelocityControllerWithRampInterface
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
index b9fbd1980f0097db0d22db4b74fbfd32f11c28aa..f528f223c63c0d5241ed5a6f75a6f93ae68c6d30 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
@@ -82,14 +82,18 @@ namespace armarx
};
/**
+ * @defgroup Library-RobotUnit-NJointControllers NJointControllers
+ * @ingroup Library-RobotUnit
+ *
+ *
* @defgroup Library-RobotUnit RobotUnit
* @ingroup RobotAPI
* A description of the library RobotUnit.
*/
/**
- * @ingroup Library-RobotUnit
- * @brief A high level controller writing its results into targes.
+ * @ingroup Library-RobotUnit-NJointControllers
+ * @brief A high level controller writing its results into targets.
*
* This is the abstract base class for all NJointControllers.
* It implements basic management routines required by the RobotUnit and some basic ice calls.
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
index 2f9f74d067902398ab44e60657b591f84efd789a..f00506d69a7c84de49b0d12ec4b20554bd2edc2e 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
@@ -49,6 +49,7 @@ namespace armarx
oriCtrl.p = cfg->p;
oriCtrl.positionPeriodLo = -M_PI;
oriCtrl.positionPeriodHi = M_PI;
+ pid.preallocate(2);
}
void NJointHolonomicPlatformRelativePositionController::rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration)
@@ -94,8 +95,8 @@ namespace armarx
// }
// ARMARX_INFO << deactivateSpam(0.1) << VAROUT(oriCtrl.currentPosition) << VAROUT(orientationError);
// ARMARX_INFO << deactivateSpam(0.1) << VAROUT(target->velocityRotation) << VAROUT(sv->velocityRotation);
- //ARMARX_RT_LOGF_INFO("current pose x: %.2f y: %2.f, error x: %.2f y: %2.f error: %2.f ori error: %2.f", currentPose[0], currentPose[1], posError[0], posError[1], posError.norm(), orientationError).deactivateSpam(0.1);
- //ARMARX_RT_LOGF_INFO("new target vel: %2.f, %2.f current vel: %2.f, %2.f", target->velocityX, target->velocityY, sv->velocityX, sv->velocityY).deactivateSpam(0.1);
+ // ARMARX_RT_LOGF_INFO("current pose x: %.2f y: %2.f, error x: %.2f y: %2.f error: %2.f ori error: %2.f", currentPose[0], currentPose[1], posError[0], posError[1], posError.norm(), orientationError).deactivateSpam(0.1);
+ // ARMARX_RT_LOGF_INFO("new target vel: %2.f, %2.f current vel: %2.f, %2.f", target->velocityX, target->velocityY, sv->velocityX, sv->velocityY).deactivateSpam(0.1);
}
void NJointHolonomicPlatformRelativePositionController::rtPreActivateController()
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
index ea191aae73cb1b22b37adbcbaafd5327ee05fc9d..997c6708e11da0b57becefa754826df3972b3b87 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
@@ -52,9 +52,9 @@ namespace armarx
float i = 0.0f;
float d = 0.0f;
float maxVelocity = 300;
- float maxAcceleration = 100;
+ float maxAcceleration = 500;
float maxRotationVelocity = 0.5;
- float maxRotationAcceleration = 0.2;
+ float maxRotationAcceleration = 0.5;
// float rad2MMFactor = 50.0f;
};
@@ -68,6 +68,11 @@ namespace armarx
};
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformRelativePositionController);
+
+ /**
+ * @brief The NJointHolonomicPlatformRelativePositionController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
class NJointHolonomicPlatformRelativePositionController:
virtual public NJointControllerWithTripleBuffer<NJointHolonomicPlatformRelativePositionControllerTarget>
{
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
index 8cc71e9189a91e84d3f6a2965a129932e0cff580..902eb7018ee4a2d7be02d440186937fb6755f9e4 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
@@ -51,6 +51,10 @@ namespace armarx
};
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformUnitVelocityPassThroughController);
+ /**
+ * @brief The NJointHolonomicPlatformUnitVelocityPassThroughController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
class NJointHolonomicPlatformUnitVelocityPassThroughController:
virtual public NJointControllerWithTripleBuffer<NJointHolonomicPlatformUnitVelocityPassThroughControllerControlData>
{
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
index 159440d3f120b7a4f1e8b3469a3849e6315c2a1f..3ed3d2e998515fd028743c1c339867f3a509eca7 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
@@ -46,6 +46,10 @@ namespace armarx
TYPEDEF_PTRS_HANDLE(NJointKinematicUnitPassThroughController);
+ /**
+ * @brief The NJointKinematicUnitPassThroughController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
class NJointKinematicUnitPassThroughController:
virtual public NJointController
{
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
index 46eff8bfb2003a4ed9216c86a57c23faaf7a2367..c0978a8eb6c7c1af87e614c0f4a8992c6538bfb1 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
@@ -63,6 +63,10 @@ namespace armarx
};
+ /**
+ * @brief The NJointTCPController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
class NJointTCPController :
public NJointControllerWithTripleBuffer<NJointTCPControllerControlData>
{
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
index 860c5e88a5f1db3cc42a582c9488b280c3459be3..743fd76d6551e4754a4b23681b81d6e403ad7261 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
@@ -42,7 +42,7 @@ namespace armarx
};
/**
* @defgroup Library-NJointTaskSpaceImpedanceController NJointTaskSpaceImpedanceController
- * @ingroup TaskSpaceActiveImpedanceControl
+ * @ingroup Library-RobotUnit-NJointControllers
* A description of the library NJointTaskSpaceImpedanceController.
*
* @class NJointTaskSpaceImpedanceController
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
index c988cec09e2d59ad93e63eb38dde3d7eeb3d5306..97d9e2c5a0950245ab6d06617136e6249cc984c0 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 = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
const SensorValueBase* sv = prov->getSensorValue(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,25 +61,33 @@ namespace armarx
if (rtGetControlStruct().trajectoryCtrl)
{
TrajectoryController& contr = *rtGetControlStruct().trajectoryCtrl;
-
- auto dimNames = contr.getTraj()->getDimensionNames();
- for (size_t i = 0; i < dimNames.size(); i++)
+ ARMARX_CHECK_EQUAL(sensors.size(), targets.size());
+ const auto& dimNames = contr.getTraj()->getDimensionNames();
+ 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[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);
+ 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);
- if (targets.count(jointName) == 1)
{
- targets[jointName]->velocity = (cfg->isPreview || finished) ? 0.0f : newVelocities(i);
+ targets.at(i)->velocity = (cfg->isPreview || finished) ? 0.0f : newVelocities(i);
}
}
@@ -167,7 +176,6 @@ namespace armarx
startTimestamp = *trajectory->getTimestamps().begin();
endTimestamp = *trajectory->getTimestamps().rbegin();
-
if (plotter)
{
StringFloatSeqDict posData;
@@ -191,6 +199,8 @@ 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));
getWriterControlStruct().trajectoryCtrl = trajectoryCtrl;
writeControlStruct();
@@ -256,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 5de238702fdc2b06dc40b84d6a9315a0e0b1fc9b..13ebf0459716b1a4725dc10cd4e12237e500a2bf 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
@@ -19,6 +19,10 @@ namespace armarx
TrajectoryControllerPtr trajectoryCtrl;
};
+ /**
+ * @brief The NJointTrajectoryController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
class NJointTrajectoryController :
public NJointControllerWithTripleBuffer<NJointTrajectoryControllerControlData>,
public NJointTrajectoryControllerInterface
@@ -47,12 +51,12 @@ 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;
Eigen::VectorXf currentPos;
bool looping = false;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index 02592203c086c583231a27d9790b763eb4daa93c..d3bdf946239c5750e4556fd871a1cbc2d8740d8c 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -81,7 +81,7 @@ namespace armarx
public:
RobotUnitEmergencyStopMaster(RobotUnit* robotUnit, std::string emergencyStopTopicName) :
robotUnit {robotUnit},
- emergencyStopTopicName {emergencyStopTopicName}
+ emergencyStopTopicName {emergencyStopTopicName}
{}
void setEmergencyStopState(EmergencyStopState state, const Ice::Current& = GlobalIceCurrent) final override
@@ -1070,13 +1070,16 @@ namespace armarx
auto start = MicroNow();
nJointCtrl->rtSwapBufferAndRun(sensorValuesTimestamp, timeSinceLastIteration);
auto duration = MicroNow() - start;
- if (duration.count() > 500)
+ if (checkControllerExecutionTimings)
{
- ARMARX_ERROR << deactivateSpam(5) << "The NJointController " << nJointCtrl->getClassName() << " took " << duration.count() << " µs to run!";
- }
- else if (duration.count() > 50)
- {
- ARMARX_RT_LOGF_WARN("An NJointController took %d µs to run!", duration.count()).deactivateSpam(5);
+ if (duration.count() > 500)
+ {
+ ARMARX_ERROR << deactivateSpam(5) << "The NJointController " << nJointCtrl->getClassName() << " took " << duration.count() << " µs to run!";
+ }
+ else if (duration.count() > 50)
+ {
+ ARMARX_RT_LOGF_WARN("The NJointController with ID %d took %d µs to run!", nJointCtrl->getId(), duration.count()).deactivateSpam(5);
+ }
}
}
}
@@ -1280,7 +1283,7 @@ namespace armarx
std::move(ctrlDeviceUsedBitmap),
std::move(ctrlDeviceUsedIndices),
deletable, internal);
- getArmarXManager()->addObjectAsync(nJointCtrl, instanceName, false, false);
+ getArmarXManager()->addObject(nJointCtrl, instanceName, false, false);
nJointControllers[instanceName] = std::move(nJointCtrl);
ARMARX_CHECK_EXPRESSION(listenerPrx);
listenerPrx->nJointControllerCreated(instanceName);
@@ -2827,9 +2830,8 @@ namespace armarx
}
- void armarx::RobotUnit::icePropertiesUpdated(const std::set<std::string>& changedProperties)
+ void armarx::RobotUnit::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
{
-
if (changedProperties.count("ObserverPublishSensorValues"))
{
ObserverPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
@@ -2860,6 +2862,8 @@ namespace armarx
ObserverPublishSensorValues = getProperty<bool>("ObserverPublishSensorValues").getValue();
ObserverPublishControlTargets = getProperty<bool>("ObserverPublishControlTargets").getValue();
+ checkControllerExecutionTimings = getProperty<bool>("CheckControllerExecutionTimings");
+
//load robot
{
robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
index 006fcf0cdf65580fab63759bd811faafd138a8f6..38ba178628e48b1f584f566661786c6e7b988767 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
@@ -240,6 +240,14 @@ namespace armarx
defineOptionalProperty<std::string>(
"TrajectoryControllerUnitName", "TrajectoryPlayer",
"Name of the TrajectoryControllerUnit. The respective component outside of the RobotUnit is TrajectoryPlayer");
+
+ // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // ////////////////////////////////////////////////////// Misc. Properties ///////////////////////////////////////////////////////////// //
+ // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+
+ defineOptionalProperty<bool>(
+ "CheckControllerExecutionTimings", false,
+ "Check controller execution timings and print a warning if execution takes long.");
}
};
@@ -593,6 +601,7 @@ namespace armarx
// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
// util
// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ bool checkControllerExecutionTimings = false;
protected:
template<class ExceptT = LogicError>
inline void throwIfStateIsNot(const std::set<RobotUnitState>& sinit, const std::string& fnc, bool onlyWarn = false) const;
@@ -626,7 +635,7 @@ namespace armarx
static constexpr std::size_t IndexSentinel();
public:
- void icePropertiesUpdated(const std::set<std::string>& changedProperties) override;
+ void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
protected:
virtual void onInitRobotUnit();
virtual void onConnectRobotUnit();
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
index fc8b362bcd2c705f163387fa4f41884d9d92ac92..fdf5ecb3e856b0758b5245197d0d79b81f940d4b 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
@@ -119,6 +119,7 @@ namespace armarx
svi.addBaseClass<SensorValue1DoFActuatorVelocity>();
svi.addBaseClass<SensorValue1DoFActuatorAcceleration>();
svi.addBaseClass<SensorValue1DoFActuatorTorque>();
+ //svi.addBaseClass<SensorValue1DoFGravityTorque>();
return svi;
}
};
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
index ac4f64be14afb2f0cde6117575622e90a7e3a1b2..e8819bfaf63860bc950194592601a4355eefd0e5 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
@@ -90,6 +90,7 @@ void armarx::PlatformSubUnit::moveTo(Ice::Float rx, Ice::Float ry, Ice::Float rr
void armarx::PlatformSubUnit::moveRelative(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current&)
{
+ relativePosCtrl->setTarget(rx, ry, rr, lac, rac);
if (!relativePosCtrl->isControllerActive())
{
relativePosCtrl->activateController();
@@ -97,7 +98,6 @@ void armarx::PlatformSubUnit::moveRelative(Ice::Float rx, Ice::Float ry, Ice::Fl
//holding the mutex here could deadlock
// std::lock_guard<std::mutex> guard {dataMutex};
ARMARX_INFO << "target orientation: " << rr;
- relativePosCtrl->setTarget(rx, ry, rr, lac, rac);
}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
index b6b0c4bcc356832158afe1a55dd48e62f5f14d84..244ebd5457cf3201e9e3d44136ca67b52620215b 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
@@ -185,8 +185,7 @@ bool TCPControllerSubUnit::isRequested(const Ice::Current&)
return false;
}
-
-void armarx::TCPControllerSubUnit::icePropertiesUpdated(const std::set<std::string>& changedProperties)
+void armarx::TCPControllerSubUnit::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
{
if (changedProperties.count("AvoidJointLimitsKp") && robotUnit)
{
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
index 9f591b3d430bc480efa896cd39c473de7c36b79f..b8911b7e5a2b05c85d0591322ffda56e6da454a8 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
@@ -108,6 +108,6 @@ namespace armarx
// PropertyUser interface
public:
- void icePropertiesUpdated(const std::set<std::string>& changedProperties) override;
+ void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
};
}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
index 7ea102ce2202e4f8c274af36574bafd7c2c5bf9f..ffefaee4d9f41c1c9cc3851a5838af1479982994 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
@@ -168,7 +168,26 @@ void TrajectoryControllerSubUnit::loadJointTraj(const TrajectoryBasePtr& jointTr
return;
}
- this->jointTraj = this->jointTraj->normalize(0, *this->jointTraj->getTimestamps().rbegin() - *this->jointTraj->getTimestamps().begin());
+ if (this->jointTraj->size() == 0)
+ {
+ ARMARX_ERROR << "Error when loading TrajectoryPlayer: trajectory is empty !!!";
+ return;
+ }
+
+ 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);
@@ -178,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();
@@ -362,7 +384,9 @@ NJointTrajectoryControllerPtr TrajectoryControllerSubUnit::createTrajectoryContr
NJointTrajectoryControllerPtr trajController = jointTrajController;
if (!controllerExists() || deleteIfAlreadyExist)
{
+ recreateController = false;
NJointTrajectoryControllerConfigPtr config = new NJointTrajectoryControllerConfig();
+ ARMARX_INFO << VAROUT(kp) << VAROUT(ki) << VAROUT(kd);
config->PID_p = kp;
config->PID_i = ki;
config->PID_d = kd;
@@ -370,7 +394,7 @@ NJointTrajectoryControllerPtr TrajectoryControllerSubUnit::createTrajectoryContr
config->jointNames = jointNames;
config->considerConstraints = considerConstraintsForTrajectoryOptimization;
config->isPreview = isPreview;
-
+ controllerName = this->getName() + "_" + "JointTrajectory" + IceUtil::generateUUID();
trajController = NJointTrajectoryControllerPtr::dynamicCast(
robotUnit->createNJointController(
"NJointTrajectoryController", controllerName,
@@ -448,3 +472,29 @@ void TrajectoryControllerSubUnit::setup(RobotUnit* rUnit)
ARMARX_CHECK_EXPRESSION(rUnit);
robotUnit = rUnit;
}
+
+
+void armarx::TrajectoryControllerSubUnit::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
+{
+ ARMARX_INFO << "Changning properties";
+ if (changedProperties.count("Kp"))
+ {
+ ARMARX_INFO << "Changning property Kp";
+ kp = getProperty<float>("Kp").getValue();
+ recreateController = true;
+ }
+
+ if (changedProperties.count("Kd"))
+ {
+ ARMARX_INFO << "Changning property Kd";
+ kd = getProperty<float>("Kd").getValue();
+ recreateController = true;
+ }
+
+ if (changedProperties.count("Ki"))
+ {
+ ARMARX_INFO << "Changning property Ki";
+ ki = getProperty<float>("Ki").getValue();
+ recreateController = true;
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
index f5c0170911f01d3c6b0c0c42fc197a56ee73d593..5153cfd298121be64c58f86eb7d02a6501b70fdd 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
@@ -41,9 +41,9 @@ namespace armarx
armarx::ComponentPropertyDefinitions(prefix)
{
defineRequiredProperty<std::string>("KinematicUnitName", "Name of the KinematicUnit. Only needed for returning to zeroFramePose while resetting.");
- defineOptionalProperty<float>("Kp", 1.f, "Proportional gain value of PID Controller");
- defineOptionalProperty<float>("Ki", 0.0f, "Integral gain value of PID Controller");
- defineOptionalProperty<float>("Kd", 0.0f, "Differential gain value of PID Controller");
+ defineOptionalProperty<float>("Kp", 1.f, "Proportional gain value of PID Controller", PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>("Ki", 0.0f, "Integral gain value of PID Controller", PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<float>("Kd", 0.0f, "Differential gain value of PID Controller", PropertyDefinitionBase::eModifiable);
defineOptionalProperty<float>("absMaximumVelocity", 80.0f, "The PID will never set a velocity above this threshold (degree/s)");
defineOptionalProperty<std::string>("CustomRootNode", "", "If this value is set, the root pose of the motion is set for this node instead of the root joint.");
@@ -162,6 +162,11 @@ namespace armarx
Mutex dataMutex;
RecursiveMutex controllerMutex;
+ bool recreateController = true;
+
+ // Component interface
+ public:
+ virtual void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
};
}
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
index 872dd12816d6f7b9fb76f4021678b29ab5e3c0bd..530c49adf230b29eb40f87e778e195927ed856fb 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.cpp
@@ -246,6 +246,12 @@ void RobotViewerWidgetController::onConnectComponent()
ui.kinematicChainComboBox->addItem(QString::fromStdString(nodeSetName));
}
ui.kinematicChainComboBox->setCurrentIndex(0);
+ ui.tcpComboBox->addItem("<default>");
+ for (auto& node : sharedRobot->getRobotNodes())
+ {
+ ui.tcpComboBox->addItem(QString::fromStdString(node));
+ }
+ ui.tcpComboBox->setCurrentIndex(0);
ui.frameComboBox->addItem("<Global>");
robotNodeNames = sharedRobot->getRobotNodes();
@@ -574,11 +580,12 @@ static std::string writeJointConfigurationToJson(VirtualRobot::Robot& robot,
}
template <typename FrameType>
-static std::string writeFramedTCP(VirtualRobot::RobotPtr const& robot, VirtualRobot::RobotNodeSetPtr const& nodeSet, std::string const& frameName)
+static std::string writeFramedTCP(VirtualRobot::RobotPtr const& robot, VirtualRobot::RobotNodeSetPtr const& nodeSet, std::string const& frameName, const std::string& tcpName)
{
+ auto tcp = tcpName.empty() ? nodeSet->getTCP() : robot->getRobotNode(tcpName);
if (nodeSet)
{
- Eigen::Matrix4f tcpMatrix = nodeSet->getTCP()->getPoseInRootFrame();
+ Eigen::Matrix4f tcpMatrix = tcp->getPoseInRootFrame();
IceInternal::Handle<FrameType> position = new FrameType(tcpMatrix, robot->getRootNode()->getName(), robot->getName());
position->changeFrame(robot, frameName);
@@ -621,7 +628,7 @@ void RobotViewerWidgetController::updateState()
// Set the locale so that the floats get converted correctly
const char* oldLocale = std::setlocale(LC_ALL, "en_US.UTF-8");
-
+ std::string tcpName = ui.tcpComboBox->currentIndex() == 0 ? "" : ui.tcpComboBox->currentText().toStdString();
std::string output;
switch (outputType)
{
@@ -630,15 +637,15 @@ void RobotViewerWidgetController::updateState()
break;
case eFramedPositionTCP:
- output = writeFramedTCP<FramedPosition>(robot, robotNodeSet, frameName);
+ output = writeFramedTCP<FramedPosition>(robot, robotNodeSet, frameName, tcpName);
break;
case eFramedOrientationTCP:
- output = writeFramedTCP<FramedOrientation>(robot, robotNodeSet, frameName);
+ output = writeFramedTCP<FramedOrientation>(robot, robotNodeSet, frameName, tcpName);
break;
case eFramedPoseTCP:
- output = writeFramedTCP<FramedPose>(robot, robotNodeSet, frameName);
+ output = writeFramedTCP<FramedPose>(robot, robotNodeSet, frameName, tcpName);
break;
default:
diff --git a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.ui b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.ui
index a2421fb314a664223e56f7372b0f23c3ca8fe01b..8450a4ec69529c37deb14cc1a9518a10978cfd3e 100644
--- a/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.ui
+++ b/source/RobotAPI/gui-plugins/RobotViewerPlugin/RobotViewerGuiPlugin.ui
@@ -28,6 +28,13 @@
<layout class="QGridLayout" name="gridLayout_2">
<item row="0" column="0">
<layout class="QGridLayout" name="gridLayout_3">
+ <item row="5" column="2">
+ <widget class="QLabel" name="label_collisionModelInflationValue">
+ <property name="text">
+ <string>0 mm</string>
+ </property>
+ </widget>
+ </item>
<item row="5" column="0">
<widget class="QLabel" name="label_collisionModelInflation">
<property name="text">
@@ -35,13 +42,10 @@
</property>
</widget>
</item>
- <item row="8" column="0" colspan="3">
- <widget class="QComboBox" name="kinematicChainComboBox"/>
- </item>
- <item row="0" column="2">
- <widget class="QCheckBox" name="cbDebugLayer">
+ <item row="1" column="2">
+ <widget class="QCheckBox" name="cbRobot">
<property name="text">
- <string>show debug layer</string>
+ <string>show robot</string>
</property>
<property name="checked">
<bool>true</bool>
@@ -49,70 +53,66 @@
</widget>
</item>
<item row="10" column="0" colspan="3">
- <widget class="QComboBox" name="frameComboBox"/>
- </item>
- <item row="7" column="0" colspan="3">
- <widget class="QLabel" name="chooseKinematicChainLabel">
+ <widget class="QLabel" name="chooseFrameLabel">
<property name="text">
- <string>Choose the kinematic chain:</string>
+ <string>Choose a frame for the coordinates:</string>
</property>
</widget>
</item>
- <item row="13" column="2">
- <widget class="QPushButton" name="copyToClipboardButton">
- <property name="text">
- <string>Copy to Clipboard</string>
- </property>
- </widget>
+ <item row="13" column="0" colspan="3">
+ <widget class="QComboBox" name="outputTypeComboBox"/>
</item>
- <item row="2" column="2">
- <widget class="QCheckBox" name="cbRoot">
- <property name="text">
- <string>show root</string>
+ <item row="15" column="0" colspan="3">
+ <widget class="QPlainTextEdit" name="previewTextBox"/>
+ </item>
+ <item row="5" column="1">
+ <widget class="QSlider" name="horizontalSliderCollisionModelInflation">
+ <property name="enabled">
+ <bool>false</bool>
</property>
- <property name="checked">
- <bool>true</bool>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="tickPosition">
+ <enum>QSlider::TicksBelow</enum>
</property>
</widget>
</item>
- <item row="9" column="0" colspan="3">
- <widget class="QLabel" name="chooseFrameLabel">
+ <item row="14" column="0">
+ <widget class="QLabel" name="previewLabel">
<property name="text">
- <string>Choose a frame for the coordinates:</string>
+ <string>Preview:</string>
</property>
</widget>
</item>
- <item row="14" column="0" colspan="3">
- <widget class="QPlainTextEdit" name="previewTextBox"/>
- </item>
- <item row="11" column="0" colspan="3">
- <widget class="QLabel" name="chooseOutputTypeLabel">
+ <item row="0" column="2">
+ <widget class="QCheckBox" name="cbDebugLayer">
<property name="text">
- <string>Choose the output type:</string>
+ <string>show debug layer</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
</property>
</widget>
</item>
- <item row="13" column="0">
- <widget class="QLabel" name="previewLabel">
+ <item row="12" column="0" colspan="3">
+ <widget class="QLabel" name="chooseOutputTypeLabel">
<property name="text">
- <string>Preview:</string>
+ <string>Choose the output type:</string>
</property>
</widget>
</item>
- <item row="12" column="0" colspan="3">
- <widget class="QComboBox" name="outputTypeComboBox"/>
- </item>
- <item row="1" column="2">
- <widget class="QCheckBox" name="cbRobot">
+ <item row="2" column="2">
+ <widget class="QCheckBox" name="cbRoot">
<property name="text">
- <string>show robot</string>
+ <string>show root</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
- <item row="13" column="1">
+ <item row="14" column="1">
<widget class="QCheckBox" name="autoUpdateCheckBox">
<property name="text">
<string>Auto Update</string>
@@ -122,25 +122,19 @@
</property>
</widget>
</item>
- <item row="0" column="0" colspan="2">
- <layout class="QVBoxLayout" name="verticalLayout">
- <property name="sizeConstraint">
- <enum>QLayout::SetMaximumSize</enum>
+ <item row="2" column="0" colspan="2">
+ <widget class="QLineEdit" name="leRobotInfo">
+ <property name="enabled">
+ <bool>false</bool>
</property>
- <item>
- <widget class="QLabel" name="labelRobotViewerName">
- <property name="font">
- <font>
- <family>AlArabiya</family>
- <pointsize>14</pointsize>
- </font>
- </property>
- <property name="text">
- <string>RobotViewer</string>
- </property>
- </widget>
- </item>
- </layout>
+ </widget>
+ </item>
+ <item row="14" column="2">
+ <widget class="QPushButton" name="copyToClipboardButton">
+ <property name="text">
+ <string>Copy to Clipboard</string>
+ </property>
+ </widget>
</item>
<item row="1" column="0" colspan="2">
<layout class="QHBoxLayout" name="horizontalLayout">
@@ -163,33 +157,49 @@
</item>
</layout>
</item>
- <item row="2" column="0" colspan="2">
- <widget class="QLineEdit" name="leRobotInfo">
- <property name="enabled">
- <bool>false</bool>
+ <item row="0" column="0" colspan="2">
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <property name="sizeConstraint">
+ <enum>QLayout::SetMaximumSize</enum>
</property>
- </widget>
+ <item>
+ <widget class="QLabel" name="labelRobotViewerName">
+ <property name="font">
+ <font>
+ <family>AlArabiya</family>
+ <pointsize>14</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>RobotViewer</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
</item>
- <item row="5" column="1">
- <widget class="QSlider" name="horizontalSliderCollisionModelInflation">
- <property name="enabled">
- <bool>false</bool>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="tickPosition">
- <enum>QSlider::TicksBelow</enum>
+ <item row="11" column="0" colspan="3">
+ <widget class="QComboBox" name="frameComboBox"/>
+ </item>
+ <item row="8" column="0" colspan="2">
+ <widget class="QLabel" name="chooseKinematicChainLabel">
+ <property name="text">
+ <string>Choose the kinematic chain:</string>
</property>
</widget>
</item>
- <item row="5" column="2">
- <widget class="QLabel" name="label_collisionModelInflationValue">
+ <item row="8" column="2">
+ <widget class="QLabel" name="label">
<property name="text">
- <string>0 mm</string>
+ <string>Choose the TCP:</string>
</property>
</widget>
</item>
+ <item row="9" column="0" colspan="2">
+ <widget class="QComboBox" name="kinematicChainComboBox"/>
+ </item>
+ <item row="9" column="2">
+ <widget class="QComboBox" name="tcpComboBox"/>
+ </item>
</layout>
</item>
</layout>
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/interface/visualization/DebugDrawerInterface.ice b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
index 1a342947e2210b180d2416b14e3bc78deedd72e8..d07c6eb97eeb1dd8624823d36cea7065451a868f 100644
--- a/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
+++ b/source/RobotAPI/interface/visualization/DebugDrawerInterface.ice
@@ -113,6 +113,13 @@ module armarx
int normalID;
int colorID;
};
+
+ struct DebugDrawerNormal
+ {
+ float x;
+ float y;
+ float z;
+ };
/*!
* \brief A triangle face consisting of 3 position-normal-color-vertices.
@@ -122,6 +129,7 @@ module armarx
DebugDrawerVertexID vertex1;
DebugDrawerVertexID vertex2;
DebugDrawerVertexID vertex3;
+ DebugDrawerNormal normal;
};
sequence<DebugDrawerFace> DebugDrawerFaceSequence;
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index b0a1a80cab68dc9f10e178cc00da1e1b65b49c94..12b46048fadfc49819e2bcf46d6bc8f18d916bb5 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -6,3 +6,5 @@ add_subdirectory(RobotAPIVariantWidget)
add_subdirectory(RobotAPINJointControllers)
add_subdirectory(DMPController)
+
+add_subdirectory(RobotStatechartHelpers)
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
index c62ad28e2a932a29032642d0230591b23f23fd29..96a17a18d61112000c44305488964218bf53b18e 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.h
@@ -69,7 +69,7 @@ namespace armarx
/**
* @defgroup Library-TaskSpaceDMPController TaskSpaceDMPController
- * @ingroup RobotAPI
+ * @ingroup Library-RobotUnit-NJointControllers
* A description of the library TaskSpaceDMPController.
*
* @class TaskSpaceDMPController
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
index e280cb1e5c6291ee61fc7c4bf53721d1255845ef..eea49086f52eadb0ba4a06b6b00e4e1d97ffde54 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
@@ -328,8 +328,8 @@ namespace armarx
Eigen::Matrix4f leftTargetPose;
Eigen::Matrix4f rightTargetPose;
- // float leftKratio = 1.0;
- // float rightKratio = 1.0;
+ float leftKratio = 1.0;
+ float rightKratio = 1.0;
if (leaderName == "Left")
{
@@ -389,7 +389,7 @@ namespace armarx
// tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
// return tcpDesiredWrench;
- throw DMP::Exception("NJointBimanualCCDMPController::getControlWrench is not implemented yet");
+ return Eigen::Vector6f::Zero();
}
@@ -744,7 +744,7 @@ namespace armarx
while (!interfaceData.updateReadBuffer())
{
-
+ usleep(100);
}
Eigen::Matrix4f leftPose = interfaceData.getReadBuffer().currentLeftPose;
Eigen::Matrix4f rightPose = interfaceData.getReadBuffer().currentRightPose;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
index 42606fff49860a22478cff6113786f1cb3ccc301..5d4714142855c327bc4b0d1d843fd7f0dd297cf9 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.h
@@ -41,6 +41,10 @@ namespace armarx
};
+ /**
+ * @brief The NJointBimanualCCDMPController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
class NJointBimanualCCDMPController :
public NJointControllerWithTripleBuffer<NJointBimanualCCDMPControllerControlData>,
public NJointBimanualCCDMPControllerInterface
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
index a93bba871aa59cff86d1a75a8ec2ebaf391a8956..0bb2ccbf81feb128cbe160bf69479f21a4c03921 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.h
@@ -50,6 +50,10 @@ namespace armarx
}
};
+ /**
+ * @brief The NJointCCDMPController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
class NJointCCDMPController :
public NJointControllerWithTripleBuffer<NJointCCDMPControllerControlData>,
public NJointCCDMPControllerInterface
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
index d23f3ad88f69e279d6d8b509312e3b1e7220b2e3..11a70b830f5e840e903290e938442831a077ff24 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.h
@@ -32,6 +32,10 @@ namespace armarx
// float update(float dt, float error);
// };
+ /**
+ * @brief The NJointJSDMPController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
class NJointJSDMPController :
public NJointControllerWithTripleBuffer<NJointJSDMPControllerControlData>,
public NJointJointSpaceDMPControllerInterface
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h
index edc8e28af78206ef068757de4a5515d4a4e15525..2703be7f78b08d47c83a3714940880f60615fb7e 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJointSpaceDMPController.h
@@ -32,6 +32,10 @@ namespace armarx
// float update(float dt, float error);
// };
+ /**
+ * @brief The NJointJointSpaceDMPController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
class NJointJointSpaceDMPController :
public NJointControllerWithTripleBuffer<NJointJointSpaceDMPControllerControlData>,
public NJointJointSpaceDMPControllerInterface
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
index d2271b6ac1ce578540bd4e38e142bda63078409e..6164cc1ac1d546bf6fd1e393c588e71645a0fa26 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.h
@@ -50,6 +50,10 @@ namespace armarx
}
};
+ /**
+ * @brief The NJointTSDMPController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
class NJointTSDMPController :
public NJointControllerWithTripleBuffer<NJointTSDMPControllerControlData>,
public NJointTaskSpaceDMPControllerInterface
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index b1332376e1f025a8cf8c144015def710431d21f5..21db16e876446bb1ac5d7d74fd0723e5bbea64db 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -320,7 +320,7 @@ namespace armarx
while (!interfaceData.updateReadBuffer())
{
-
+ usleep(100);
}
Eigen::Matrix4f pose = interfaceData.getReadBuffer().currentTcpPose;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index 8cc6e483f4aac8d285871f2472045d99eda9280e..9d34fd56482f3951173175ae4295b0015700393f 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -31,6 +31,10 @@ namespace armarx
+ /**
+ * @brief The NJointTaskSpaceImpedanceDMPController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
class NJointTaskSpaceImpedanceDMPController :
public NJointControllerWithTripleBuffer<NJointTaskSpaceImpedanceDMPControllerControlData>,
public NJointTaskSpaceImpedanceDMPControllerInterface
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt b/source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6202780fac063d4afae5c27ddfaa183b9a8efb46
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/CMakeLists.txt
@@ -0,0 +1,46 @@
+set(LIB_NAME RobotStatechartHelpers)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+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
+ ArmarXCoreInterfaces ArmarXCore ArmarXCoreObservers ArmarXCoreStatechart
+ RobotAPIInterfaces RobotAPICore
+ ${Simox_LIBRARIES}
+)
+
+set(LIB_FILES
+#./RobotStatechartHelpers.cpp
+VelocityControllerHelper.cpp
+PositionControllerHelper.cpp
+ForceTorqueHelper.cpp
+KinematicUnitHelper.cpp
+RobotNameHelper.cpp
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.cpp
+)
+set(LIB_HEADERS
+#./RobotStatechartHelpers.h
+VelocityControllerHelper.h
+PositionControllerHelper.h
+ForceTorqueHelper.h
+KinematicUnitHelper.h
+RobotNameHelper.h
+#@TEMPLATE_LINE@@COMPONENT_PATH@/@COMPONENT_NAME@.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${COMPONENT_LIBS}")
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a170c9391c08ca232bedba7604fa637f80b9d0c0
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.cpp
@@ -0,0 +1,52 @@
+/*
+ * 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 "ForceTorqueHelper.h"
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <ArmarXCore/observers/variant/DatafieldRef.h>
+
+using namespace armarx;
+
+ForceTorqueHelper::ForceTorqueHelper(const ForceTorqueUnitObserverInterfacePrx& forceTorqueObserver, const std::string& FTDatefieldName)
+{
+ forceDf = DatafieldRefPtr::dynamicCast(forceTorqueObserver->getForceDatafield(FTDatefieldName));
+ torqueDf = DatafieldRefPtr::dynamicCast(forceTorqueObserver->getTorqueDatafield(FTDatefieldName));
+ setZero();
+}
+
+Eigen::Vector3f ForceTorqueHelper::getForce()
+{
+ return forceDf->getDataField()->get<FramedDirection>()->toEigen() - initialForce;
+}
+
+Eigen::Vector3f ForceTorqueHelper::getTorque()
+{
+ return torqueDf->getDataField()->get<FramedDirection>()->toEigen() - initialTorque;
+}
+
+void ForceTorqueHelper::setZero()
+{
+ initialForce = forceDf->getDataField()->get<FramedDirection>()->toEigen();
+ initialTorque = torqueDf->getDataField()->get<FramedDirection>()->toEigen();
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..a6ce157fcd0c3363e4245fa3afc76da476220b43
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h
@@ -0,0 +1,53 @@
+/*
+ * 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/ForceTorqueUnit.h>
+#include <Eigen/Dense>
+
+namespace armarx
+{
+ class DatafieldRef;
+ typedef IceInternal::Handle<DatafieldRef> DatafieldRefPtr;
+
+ class ForceTorqueHelper;
+ typedef boost::shared_ptr<ForceTorqueHelper> ForceTorqueHelperPtr;
+
+ class ForceTorqueHelper
+ {
+ public:
+ ForceTorqueHelper(const ForceTorqueUnitObserverInterfacePrx& forceTorqueObserver, const std::string& FTDatefieldName);
+
+ Eigen::Vector3f getForce();
+ Eigen::Vector3f getTorque();
+ void setZero();
+
+ DatafieldRefPtr forceDf;
+ DatafieldRefPtr torqueDf;
+ Eigen::Vector3f initialForce;
+ Eigen::Vector3f initialTorque;
+ };
+}
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/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a0fd892e9a748e84e6af30841e6eb22f7a36b542
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
@@ -0,0 +1,150 @@
+/*
+ * 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 "PositionControllerHelper.h"
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+using namespace armarx;
+
+PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const Eigen::Matrix4f& target)
+ : posController(tcp), velocityControllerHelper(velocityControllerHelper), currentWaypointIndex(0)
+{
+ waypoints.push_back(target);
+}
+
+PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<Eigen::Matrix4f>& waypoints)
+ : posController(tcp), velocityControllerHelper(velocityControllerHelper), waypoints(waypoints), currentWaypointIndex(0)
+{
+}
+
+PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<PosePtr>& waypoints)
+ : posController(tcp), velocityControllerHelper(velocityControllerHelper), currentWaypointIndex(0)
+{
+ for (const PosePtr& pose : waypoints)
+ {
+ this->waypoints.push_back(pose->toEigen());
+ }
+}
+
+void PositionControllerHelper::update()
+{
+ if (!isLastWaypoint() && isCurrentTargetNear())
+ {
+ currentWaypointIndex++;
+ }
+ Eigen::VectorXf cv = posController.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All);
+ velocityControllerHelper->setTargetVelocity(cv);
+}
+
+void PositionControllerHelper::setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints)
+{
+ this->waypoints = waypoints;
+ currentWaypointIndex = 0;
+}
+
+void PositionControllerHelper::setNewWaypoints(const std::vector<PosePtr>& waypoints)
+{
+ this->waypoints.clear();
+ for (const PosePtr& pose : waypoints)
+ {
+ this->waypoints.push_back(pose->toEigen());
+ }
+ currentWaypointIndex = 0;
+}
+
+void PositionControllerHelper::addWaypoint(const Eigen::Matrix4f& waypoint)
+{
+ this->waypoints.push_back(waypoint);
+}
+
+void PositionControllerHelper::setTarget(const Eigen::Matrix4f& target)
+{
+ waypoints.clear();
+ waypoints.push_back(target);
+ currentWaypointIndex = 0;
+}
+
+float PositionControllerHelper::getPositionError() const
+{
+ return posController.getPositionError(getCurrentTarget());
+}
+
+float PositionControllerHelper::getOrientationError() const
+{
+ return posController.getOrientationError(getCurrentTarget());
+}
+
+bool PositionControllerHelper::isCurrentTargetReached() const
+{
+ return getPositionError() < thresholdPositionReached && getOrientationError() < thresholdOrientationReached;
+}
+
+bool PositionControllerHelper::isCurrentTargetNear() const
+{
+ return getPositionError() < thresholdPositionNear && getOrientationError() < thresholdOrientationNear;
+}
+
+bool PositionControllerHelper::isFinalTargetReached() const
+{
+ return isLastWaypoint() && isCurrentTargetReached();
+}
+
+bool PositionControllerHelper::isFinalTargetNear() const
+{
+ return isLastWaypoint() && isCurrentTargetNear();
+}
+
+bool PositionControllerHelper::isLastWaypoint() const
+{
+ return currentWaypointIndex + 1 >= waypoints.size();
+}
+
+const Eigen::Matrix4f& PositionControllerHelper::getCurrentTarget() const
+{
+ return waypoints.at(currentWaypointIndex);
+}
+
+size_t PositionControllerHelper::skipToClosestWaypoint(float rad2mmFactor)
+{
+ float dist = FLT_MAX;
+ size_t minIndex = 0;
+ for (size_t i = 0; i < waypoints.size(); i++)
+ {
+ float posErr = posController.getPositionError(waypoints.at(i));
+ float oriErr = posController.getOrientationError(waypoints.at(i));
+ float d = posErr + oriErr * rad2mmFactor;
+ if (d < dist)
+ {
+ minIndex = i;
+ dist = d;
+ }
+ }
+ currentWaypointIndex = minIndex;
+ return currentWaypointIndex;
+}
+
+void PositionControllerHelper::setNullSpaceControl(bool enabled)
+{
+ velocityControllerHelper->setNullSpaceControl(enabled);
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..b1ccef061e7bd34e0bc6f9eaa7809b79783da32f
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
@@ -0,0 +1,84 @@
+/*
+ * 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 <Eigen/Dense>
+
+#include "VelocityControllerHelper.h"
+
+#include <VirtualRobot/Robot.h>
+
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
+namespace armarx
+{
+ class PositionControllerHelper;
+ typedef boost::shared_ptr<PositionControllerHelper> PositionControllerHelperPtr;
+
+ class PositionControllerHelper
+ {
+ public:
+ PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const Eigen::Matrix4f& target);
+ PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<Eigen::Matrix4f>& waypoints);
+ PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<PosePtr>& waypoints);
+
+ void update();
+
+ void setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints);
+ void setNewWaypoints(const std::vector<PosePtr>& waypoints);
+ void addWaypoint(const Eigen::Matrix4f& waypoint);
+ void setTarget(const Eigen::Matrix4f& target);
+
+ float getPositionError() const;
+
+ float getOrientationError() const;
+
+ bool isCurrentTargetReached() const;
+ bool isCurrentTargetNear() const;
+ bool isFinalTargetReached() const;
+ bool isFinalTargetNear() const;
+
+ bool isLastWaypoint() const;
+
+ const Eigen::Matrix4f& getCurrentTarget() const;
+
+ size_t skipToClosestWaypoint(float rad2mmFactor);
+
+ void setNullSpaceControl(bool enabled);
+
+ CartesianPositionController posController;
+ VelocityControllerHelperPtr velocityControllerHelper;
+
+ std::vector<Eigen::Matrix4f> waypoints;
+ size_t currentWaypointIndex = 0;
+
+ float thresholdPositionReached = 0;
+ float thresholdOrientationReached = 0;
+ float thresholdPositionNear = 0;
+ float thresholdOrientationNear = 0;
+ };
+}
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/RobotStatechartHelpers/RobotStatechartHelpers.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..720addebc816a025b5ca6fef106dedb2df718f6f
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.cpp
@@ -0,0 +1,28 @@
+/*
+ * 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::ArmarXObjects::RobotStatechartHelpers
+ * @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 "RobotStatechartHelpers.h"
+
+
+using namespace armarx;
+
+
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.h
new file mode 100644
index 0000000000000000000000000000000000000000..c476160c84e8ea782ff55c10f4ca890560abe550
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotStatechartHelpers.h
@@ -0,0 +1,45 @@
+/*
+ * 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::ArmarXObjects::RobotStatechartHelpers
+ * @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
+
+
+namespace armarx
+{
+ /**
+ * @defgroup Library-RobotStatechartHelpers RobotStatechartHelpers
+ * @ingroup RobotAPI
+ * A description of the library RobotStatechartHelpers.
+ *
+ * @class RobotStatechartHelpers
+ * @ingroup Library-RobotStatechartHelpers
+ * @brief Brief description of class RobotStatechartHelpers.
+ *
+ * Detailed description of class RobotStatechartHelpers.
+ */
+ class RobotStatechartHelpers
+ {
+ public:
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..82caa574643aa2302f0f88a9278f38343e14d28f
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
@@ -0,0 +1,91 @@
+/*
+ * 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 "VelocityControllerHelper.h"
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+using namespace armarx;
+
+
+VelocityControllerHelper::VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, const std::string& nodeSetName, const std::string& controllerName)
+ : robotUnit(robotUnit), controllerName(controllerName)
+{
+ config = new NJointCartesianVelocityControllerWithRampConfig(nodeSetName, "", CartesianSelectionMode::eAll, 500, 1, 2, 1, 2);
+ init();
+}
+
+VelocityControllerHelper::VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, NJointCartesianVelocityControllerWithRampConfigPtr config, const std::string& controllerName)
+ : robotUnit(robotUnit), controllerName(controllerName)
+{
+ this->config = config;
+ init();
+}
+
+void VelocityControllerHelper::init()
+{
+ NJointControllerInterfacePrx ctrl = robotUnit->getNJointController(controllerName);
+ if (ctrl)
+ {
+ controllerCreated = false;
+ }
+ else
+ {
+ ctrl = robotUnit->createNJointController("NJointCartesianVelocityControllerWithRamp", controllerName, config);
+ controllerCreated = true;
+ }
+ controller = NJointCartesianVelocityControllerWithRampInterfacePrx::checkedCast(ctrl);
+ controller->activateController();
+}
+
+void VelocityControllerHelper::setTargetVelocity(const Eigen::VectorXf& cv)
+{
+ controller->setTargetVelocity(cv(0), cv(1), cv(2), cv(3), cv(4), cv(5));
+}
+
+void VelocityControllerHelper::setNullSpaceControl(bool enabled)
+{
+ if (enabled)
+ {
+ controller->setJointLimitAvoidanceScale(config->jointLimitAvoidanceScale);
+ controller->setKpJointLimitAvoidance(config->KpJointLimitAvoidance);
+ }
+ else
+ {
+ controller->setJointLimitAvoidanceScale(0);
+ controller->setKpJointLimitAvoidance(0);
+ }
+}
+
+void VelocityControllerHelper::cleanup()
+{
+ controller->deactivateController();
+ if (controllerCreated)
+ {
+ while (controller->isControllerActive())
+ {
+ TimeUtil::SleepMS(1);
+ }
+ controller->deleteController();
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..b2f2ce30e0341a1a17ce6c4c1484af9743300b74
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h
@@ -0,0 +1,60 @@
+/*
+ * 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/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+#include <Eigen/Dense>
+
+namespace armarx
+{
+ class VelocityControllerHelper;
+ typedef boost::shared_ptr<VelocityControllerHelper> VelocityControllerHelperPtr;
+
+ class VelocityControllerHelper
+ {
+ public:
+ VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, const std::string& nodeSetName, const std::string& controllerName);
+ VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, NJointCartesianVelocityControllerWithRampConfigPtr config, const std::string& controllerName);
+
+ void init();
+
+ void setTargetVelocity(const Eigen::VectorXf& cv);
+
+ void setNullSpaceControl(bool enabled);
+
+ void cleanup();
+
+ NJointCartesianVelocityControllerWithRampConfigPtr config;
+ NJointCartesianVelocityControllerWithRampInterfacePrx controller;
+ RobotUnitInterfacePrx robotUnit;
+ std::string controllerName;
+ bool controllerCreated = false;
+
+ float initialKp;
+ };
+}
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index 167b6be46782974f9ea928c121163f4565ad7193..de555d6f62ccbc533487c91e37a55440947b1f7f 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -29,6 +29,7 @@ set(LIB_FILES
OrientedPoint.cpp
FramedOrientedPoint.cpp
RobotStatechartContext.cpp
+ RobotPool.cpp
checks/ConditionCheckMagnitudeChecks.cpp
observerfilters/PoseAverageFilter.cpp
observerfilters/PoseMedianOffsetFilter.cpp
@@ -50,6 +51,7 @@ set(LIB_FILES
set(LIB_HEADERS
EigenStl.h
PIDController.h
+ MultiDimPIDController.h
Trajectory.h
TrajectoryController.h
VectorHelpers.h
@@ -59,6 +61,7 @@ set(LIB_HEADERS
OrientedPoint.h
FramedOrientedPoint.h
RobotStatechartContext.h
+ RobotPool.h
observerfilters/PoseMedianFilter.h
observerfilters/PoseAverageFilter.h
observerfilters/PoseMedianOffsetFilter.h
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index 1c49e462dee3383b6ac73cdeca27d5f16a4388bc..3ffd160461bef2710fc8041f66cf3d8588819fd4 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -32,7 +32,7 @@ CartesianPositionController::CartesianPositionController(const VirtualRobot::Rob
{
}
-Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode)
+Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const
{
int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
@@ -68,14 +68,14 @@ Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& ta
return cartesianVel;
}
-float CartesianPositionController::getPositionError(const Eigen::Matrix4f& targetPose)
+float CartesianPositionController::getPositionError(const Eigen::Matrix4f& targetPose) const
{
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
return errPos.norm();
}
-float CartesianPositionController::getOrientationError(const Eigen::Matrix4f& targetPose)
+float CartesianPositionController::getOrientationError(const Eigen::Matrix4f& targetPose) const
{
Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
@@ -84,14 +84,14 @@ float CartesianPositionController::getOrientationError(const Eigen::Matrix4f& ta
return aa.angle();
}
-Eigen::Vector3f CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose)
+Eigen::Vector3f CartesianPositionController::getPositionDiff(const Eigen::Matrix4f& targetPose) const
{
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
return targetPos - tcp->getPositionInRootFrame();
}
-Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Matrix4f& targetPose)
+Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Matrix4f& targetPose) const
{
Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.h b/source/RobotAPI/libraries/core/CartesianPositionController.h
index 9d6843006f86c38460153a67ad44b7d4055cc15d..e0c4ec59905a5672a93e19bb11f4dc0eb7cd4338 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.h
@@ -40,12 +40,12 @@ namespace armarx
public:
CartesianPositionController(const VirtualRobot::RobotNodePtr& tcp);
- Eigen::VectorXf calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode);
+ Eigen::VectorXf calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const;
- float getPositionError(const Eigen::Matrix4f& targetPose);
- float getOrientationError(const Eigen::Matrix4f& targetPose);
- Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose);
- Eigen::Vector3f getOrientationDiff(const Eigen::Matrix4f& targetPose);
+ float getPositionError(const Eigen::Matrix4f& targetPose) const;
+ float getOrientationError(const Eigen::Matrix4f& targetPose) const;
+ Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose) const;
+ Eigen::Vector3f getOrientationDiff(const Eigen::Matrix4f& targetPose) const;
//CartesianVelocityController velocityController;
float KpPos = 1;
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
index cdb5fc3666a89f134f59772f13a0b65c2bb99c66..5ba9cd5d3f6b764985655d70a55de54161ab87f0 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityController.cpp
@@ -57,19 +57,28 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
//ARMARX_IMPORTANT << jacobi;
inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
+ // ARMARX_IMPORTANT << "nullspaceVel " << nullspaceVel.transpose();
Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
//ARMARX_IMPORTANT << "The rank of the jacobi is " << lu_decomp.rank();
//ARMARX_IMPORTANT << "Null space: " << lu_decomp.kernel();
Eigen::MatrixXf nullspace = lu_decomp.kernel();
+
+
+ // Eigen::MatrixXf nullspace = Eigen::MatrixXf::Identity(jacobi.cols(), jacobi.cols()) - inv * jacobi;
+
Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
for (int i = 0; i < nullspace.cols(); i++)
{
nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
}
+ // Eigen::VectorXf nsv = nullspace * nullspaceVel;
+
//nsv /= kernel.cols();
+
Eigen::VectorXf jointVel = inv * cartesianVel;
+
jointVel += nsv;
return jointVel;
@@ -98,7 +107,9 @@ Eigen::VectorXf CartesianVelocityController::calculateNullspaceVelocity(const Ei
{
Eigen::VectorXf regularization = calculateRegularization(mode);
Eigen::VectorXf vel = cartesianVel * regularization;
+
return KpScale * vel.norm() * calculateJointLimitAvoidance();
+
}
void CartesianVelocityController::setCartesianRegularization(float cartesianMMRegularization, float cartesianRadianRegularization)
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
index 264db7d88a2101edcf3d674e3ccfa4abbfae8734..e1d5154f4e51f7b959b1c891c63cdccbaca4f23b 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
@@ -23,6 +23,8 @@
#include "CartesianVelocityControllerWithRamp.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
using namespace armarx;
CartesianVelocityControllerWithRamp::CartesianVelocityControllerWithRamp(const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode,
@@ -36,17 +38,20 @@ CartesianVelocityControllerWithRamp::CartesianVelocityControllerWithRamp(const V
void CartesianVelocityControllerWithRamp::setCurrentJointVelocity(const Eigen::VectorXf& currentJointVelocity)
{
Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller.tcp, mode);
+ Eigen::MatrixXf inv = controller.ik->computePseudoInverseJacobianMatrix(jacobi, controller.ik->getJacobiRegularization(mode));
+
Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
Eigen::MatrixXf nullspace = lu_decomp.kernel();
Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
+
for (int i = 0; i < nullspace.cols(); i++)
{
nsv += nullspace.col(i) * nullspace.col(i).dot(currentJointVelocity) / nullspace.col(i).squaredNorm();
}
-
cartesianVelocityRamp.setState(jacobi * currentJointVelocity, mode);
nullSpaceVelocityRamp.setState(nsv);
+
}
Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt)
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
index b5ed838bd322bd78581b68c52328454d3556dcb9..21cc73a781df9261d33afe050728680e7d88103d 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityRamp.cpp
@@ -23,6 +23,8 @@
#include "CartesianVelocityRamp.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
using namespace armarx;
CartesianVelocityRamp::CartesianVelocityRamp()
@@ -60,6 +62,7 @@ Eigen::VectorXf CartesianVelocityRamp::update(const Eigen::VectorXf& target, flo
}
state += delta / factor;
+ // ARMARX_IMPORTANT << "CartRamp state " << state;
return state;
}
diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index 0cd53f085c58c3359d439fbfeacd2b50ba77560a..ff6bcc0ed75847d058ee63d1abc49ca8e614c511 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -220,7 +220,7 @@ namespace armarx
string FramedDirection::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
+ s << toEigen().format(ArmarXEigenFormat) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
return s.str();
}
@@ -318,7 +318,8 @@ namespace armarx
string FramedPose::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
+ const Eigen::IOFormat ArmarXEigenFormat(Eigen::StreamPrecision, 4, " ", "\n", "", "");
+ s << toEigen().format(ArmarXEigenFormat) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
return s.str();
}
@@ -658,7 +659,7 @@ namespace armarx
string FramedPosition::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
+ s << toEigen().format(ArmarXEigenFormat) << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
return s.str();
}
@@ -728,7 +729,7 @@ namespace armarx
string FramedOrientation::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen() << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
+ s << toEigen()/*.format(ArmarXEigenFormat)*/ << std::endl << "frame: " << getFrame() << (agent.empty() ? "" : (" agent: " + agent));
return s.str();
}
diff --git a/source/RobotAPI/libraries/core/JointVelocityRamp.cpp b/source/RobotAPI/libraries/core/JointVelocityRamp.cpp
index f0488c9e3e4c743b1add06c1e9bbe13cf7a92ebc..33128cf1f5f1f360cc163ad07aff60cf99f4afdb 100644
--- a/source/RobotAPI/libraries/core/JointVelocityRamp.cpp
+++ b/source/RobotAPI/libraries/core/JointVelocityRamp.cpp
@@ -22,6 +22,7 @@
*/
#include "JointVelocityRamp.h"
+#include <ArmarXCore/core/logging/Logging.h>
using namespace armarx;
@@ -36,13 +37,18 @@ void JointVelocityRamp::setState(const Eigen::VectorXf& state)
Eigen::VectorXf JointVelocityRamp::update(const Eigen::VectorXf& target, float dt)
{
+ if (dt <= 0)
+ {
+ return state;
+ }
Eigen::VectorXf delta = target - state;
- float factor = delta.norm() / maxAcceleration;
+ float factor = delta.norm() / maxAcceleration / dt;
factor = std::max(factor, 1.f);
- state += delta / factor * dt;
+ state += delta / factor;
+ // ARMARX_IMPORTANT << "JointRamp state " << state;
return state;
}
diff --git a/source/RobotAPI/libraries/core/MultiDimPIDController.h b/source/RobotAPI/libraries/core/MultiDimPIDController.h
new file mode 100644
index 0000000000000000000000000000000000000000..4186cb9b15f0134e0d876194924dfe97fb7a735c
--- /dev/null
+++ b/source/RobotAPI/libraries/core/MultiDimPIDController.h
@@ -0,0 +1,245 @@
+/*
+ * 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 <ArmarXCore/core/logging/Logging.h>
+#include <Eigen/Core>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
+
+namespace armarx
+{
+ template <int dimensions = Eigen::Dynamic>
+ class MultiDimPIDControllerTemplate :
+ public Logging
+ {
+ public:
+ typedef Eigen::Matrix<float, dimensions, 1> PIDVectorX;
+
+ MultiDimPIDControllerTemplate(float Kp,
+ float Ki,
+ float Kd,
+ double maxControlValue = std::numeric_limits<double>::max(),
+ double maxDerivation = std::numeric_limits<double>::max(),
+ bool threadSafe = true,
+ std::vector<bool> limitless = {}) :
+ Kp(Kp),
+ Ki(Ki),
+ Kd(Kd),
+ integral(0),
+ derivative(0),
+ previousError(0),
+ maxControlValue(maxControlValue),
+ maxDerivation(maxDerivation),
+ threadSafe(threadSafe),
+ limitless(limitless)
+ {
+ reset();
+ }
+
+ void preallocate(size_t size)
+ {
+ stackAllocations.zeroVec = PIDVectorX::Zero(size);
+ stackAllocations.errorVec = stackAllocations.zeroVec;
+ stackAllocations.direction = stackAllocations.zeroVec;
+ stackAllocations.oldControlValue = stackAllocations.zeroVec;
+ }
+
+ ~MultiDimPIDControllerTemplate() {}
+ 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;
+
+ stackAllocations.errorVec = target - processValue;
+
+ if (limitless.size() != 0)
+ {
+ ARMARX_CHECK_EQUAL(limitless.size(), (size_t)stackAllocations.errorVec.rows());
+ for (size_t i = 0; i < limitless.size(); i++)
+ {
+ if (limitless.at(i))
+ {
+ stackAllocations.errorVec(i) = math::MathUtils::angleModPI(stackAllocations.errorVec(i));
+ }
+ }
+ }
+
+
+ double error = stackAllocations.errorVec.norm();
+
+ //double dt = (now - lastUpdateTime).toSecondsDouble();
+ // ARMARX_INFO << deactivateSpam() << VAROUT(dt);
+ if (!firstRun)
+ {
+ integral += error * deltaSec;
+ integral = std::min(integral, maxIntegral);
+ if (deltaSec > 0.0)
+ {
+ derivative = (error - previousError) / deltaSec;
+ }
+ }
+
+ firstRun = false;
+ stackAllocations.direction = targetValue; // copy size
+
+ if (error > 0)
+ {
+ stackAllocations.direction = stackAllocations.errorVec.normalized();
+ }
+ else
+ {
+ stackAllocations.direction.setZero();
+ }
+
+ if (controlValue.rows() > 0)
+ {
+ stackAllocations.oldControlValue = controlValue;
+ }
+ else
+ {
+ stackAllocations.oldControlValue = stackAllocations.zeroVec;
+ }
+ controlValue = stackAllocations.direction * (Kp * error + Ki * integral + Kd * derivative);
+
+ if (deltaSec > 0.0)
+ {
+ PIDVectorX accVec = (controlValue - stackAllocations.oldControlValue) / deltaSec;
+ float maxNewJointAcc = accVec.maxCoeff();
+ float minNewJointAcc = accVec.minCoeff();
+ maxNewJointAcc = std::max<float>(fabs(minNewJointAcc), fabs(maxNewJointAcc));
+ if (maxNewJointAcc > maxDerivation)
+ {
+ auto newValue = stackAllocations.oldControlValue + accVec * maxDerivation / maxNewJointAcc * deltaSec;
+ ARMARX_DEBUG << deactivateSpam(0.5) << VAROUT(maxDerivation) << VAROUT(maxNewJointAcc) << VAROUT(controlValue) << VAROUT(stackAllocations.oldControlValue) << VAROUT(newValue);
+ controlValue = newValue;
+ }
+ }
+
+
+ float max = controlValue.maxCoeff();
+ float min = controlValue.minCoeff();
+ max = std::max<float>(fabs(min), fabs(max));
+
+
+
+ if (max > maxControlValue)
+ {
+ auto newValue = controlValue * maxControlValue / max;
+ ARMARX_DEBUG << deactivateSpam(0.5) << " Control value to big: " << controlValue << " max value: " << maxControlValue << " new value: " << newValue;
+ controlValue = newValue;
+ }
+ ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) << " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
+
+ previousError = error;
+ lastUpdateTime += IceUtil::Time::seconds(deltaSec);
+
+ }
+ void update(const PIDVectorX& measuredValue, const PIDVectorX& targetValue)
+ {
+ ScopedRecursiveLockPtr lock = getLock();
+ IceUtil::Time now = TimeUtil::GetTime();
+
+ if (firstRun)
+ {
+ lastUpdateTime = TimeUtil::GetTime();
+ }
+
+ double dt = (now - lastUpdateTime).toSecondsDouble();
+ update(dt, measuredValue, targetValue);
+ lastUpdateTime = now;
+ }
+ const PIDVectorX&
+ getControlValue() const
+ {
+ return controlValue;
+ }
+ void setMaxControlValue(double value)
+ {
+ ScopedRecursiveLockPtr lock = getLock();
+ maxControlValue = value;
+ }
+
+ void reset()
+ {
+ ScopedRecursiveLockPtr lock = getLock();
+ firstRun = true;
+ previousError = 0;
+ integral = 0;
+ lastUpdateTime = TimeUtil::GetTime();
+ // controlValue.setZero();
+ // processValue.setZero();
+ // target.setZero();
+
+
+ }
+ // protected:
+ float Kp, Ki, Kd;
+ double integral;
+ double maxIntegral = std::numeric_limits<double>::max();
+ double derivative;
+ double previousError;
+ PIDVectorX processValue;
+ PIDVectorX target;
+ IceUtil::Time lastUpdateTime;
+ PIDVectorX controlValue;
+ double maxControlValue;
+ double maxDerivation;
+ bool firstRun;
+ mutable RecursiveMutex mutex;
+ bool threadSafe = true;
+ std::vector<bool> limitless;
+ private:
+
+ struct StackAllocationHelper
+ {
+ PIDVectorX errorVec;
+ PIDVectorX direction;
+ PIDVectorX oldControlValue;
+ PIDVectorX zeroVec;
+ } stackAllocations;
+
+ ScopedRecursiveLockPtr getLock() const
+ {
+ if (threadSafe)
+ {
+ return ScopedRecursiveLockPtr(new ScopedRecursiveLock(mutex));
+ }
+ else
+ {
+ return ScopedRecursiveLockPtr();
+ }
+ }
+ };
+ typedef MultiDimPIDControllerTemplate<> MultiDimPIDController;
+ typedef boost::shared_ptr<MultiDimPIDControllerTemplate<>> MultiDimPIDControllerPtr;
+}
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index 928e8451aee81823951866979f5d0a30321ca70b..a2ae65747f82216d6c57ea52738d7df32a340482 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -25,7 +25,6 @@
#include "PIDController.h"
#include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
using namespace armarx;
@@ -128,7 +127,7 @@ void PIDController::update(double deltaSec, double measuredValue, double targetV
if (!firstRun)
{
integral += error * deltaSec;
-
+ integral = math::MathUtils::LimitTo(integral, maxIntegral);
if (deltaSec > 0.0)
{
derivative = (error - previousError) / deltaSec;
@@ -162,158 +161,16 @@ double PIDController::getControlValue() const
}
-MultiDimPIDController::MultiDimPIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation, bool threadSafe, std::vector<bool> limitless) :
- Kp(Kp),
- Ki(Ki),
- Kd(Kd),
- integral(0),
- derivative(0),
- previousError(0),
- maxControlValue(maxControlValue),
- maxDerivation(maxDerivation),
- threadSafe(threadSafe),
- limitless(limitless)
-{
- reset();
-}
-
-void MultiDimPIDController::update(const double deltaSec, const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue)
-{
- ScopedRecursiveLockPtr lock = getLock();
- processValue = measuredValue;
- target = targetValue;
-
- Eigen::VectorXf errorVec = target - processValue;
-
- if (limitless.size() != 0)
- {
- ARMARX_CHECK_EQUAL(limitless.size(), (size_t)errorVec.rows());
- for (size_t i = 0; i < limitless.size(); i++)
- {
- if (limitless.at(i))
- {
- errorVec(i) = math::MathUtils::angleModPI(errorVec(i));
- }
- }
- }
-
-
- double error = errorVec.norm();
-
- //double dt = (now - lastUpdateTime).toSecondsDouble();
- // ARMARX_INFO << deactivateSpam() << VAROUT(dt);
- if (!firstRun)
- {
- integral += error * deltaSec;
-
- if (deltaSec > 0.0)
- {
- derivative = (error - previousError) / deltaSec;
- }
- }
-
- firstRun = false;
- Eigen::VectorXf direction = targetValue; // copy size
-
- if (error > 0)
- {
- direction = errorVec.normalized();
- }
- else
- {
- direction.setZero();
- }
- Eigen::VectorXf oldControlValue;
- if (controlValue.rows() > 0)
- {
- oldControlValue = controlValue;
- }
- else
- {
- oldControlValue = Eigen::VectorXf::Zero(measuredValue.rows());
- }
- controlValue = direction * (Kp * error + Ki * integral + Kd * derivative);
-
- if (deltaSec > 0.0)
- {
- Eigen::VectorXf accVec = (controlValue - oldControlValue) / deltaSec;
- float maxNewJointAcc = accVec.maxCoeff();
- float minNewJointAcc = accVec.minCoeff();
- maxNewJointAcc = std::max<float>(fabs(minNewJointAcc), fabs(maxNewJointAcc));
- if (maxNewJointAcc > maxDerivation)
- {
- auto newValue = oldControlValue + accVec * maxDerivation / maxNewJointAcc * deltaSec;
- ARMARX_DEBUG << deactivateSpam(0.5) << VAROUT(maxDerivation) << VAROUT(maxNewJointAcc) << VAROUT(controlValue) << VAROUT(oldControlValue) << VAROUT(newValue);
- controlValue = newValue;
- }
- }
-
- float max = controlValue.maxCoeff();
- float min = controlValue.minCoeff();
- max = std::max<float>(fabs(min), fabs(max));
- if (max > maxControlValue)
- {
- auto newValue = controlValue * maxControlValue / max;
- ARMARX_DEBUG << deactivateSpam(0.5) << " Control value to big: " << controlValue << " max value: " << maxControlValue << " new value: " << newValue;
- controlValue = newValue;
- }
- ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) << " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
- previousError = error;
- lastUpdateTime += IceUtil::Time::seconds(deltaSec);
-}
-void MultiDimPIDController::update(const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue)
-{
- ScopedRecursiveLockPtr lock = getLock();
- IceUtil::Time now = TimeUtil::GetTime();
- if (firstRun)
- {
- lastUpdateTime = TimeUtil::GetTime();
- }
- double dt = (now - lastUpdateTime).toSecondsDouble();
- update(dt, measuredValue, targetValue);
- lastUpdateTime = now;
-}
-const Eigen::VectorXf& MultiDimPIDController::getControlValue() const
-{
- return controlValue;
-}
-void MultiDimPIDController::reset()
-{
- ScopedRecursiveLockPtr lock = getLock();
- firstRun = true;
- previousError = 0;
- integral = 0;
- lastUpdateTime = TimeUtil::GetTime();
- // controlValue.setZero();
- // processValue.setZero();
- // target.setZero();
-}
-ScopedRecursiveLockPtr MultiDimPIDController::getLock() const
-{
- if (threadSafe)
- {
- return ScopedRecursiveLockPtr(new ScopedRecursiveLock(mutex));
- }
- else
- {
- return ScopedRecursiveLockPtr();
- }
-}
-void MultiDimPIDController::setMaxControlValue(double value)
-{
- ScopedRecursiveLockPtr lock = getLock();
- maxControlValue = value;
-}
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index e6e9976b94e31241d086ee513f117d8b3aa81fed..f0409a76531fac273ca193bc3b844ab3b2926985 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <Eigen/Core>
+#include "MultiDimPIDController.h"
namespace armarx
{
@@ -51,6 +52,7 @@ namespace armarx
// protected:
float Kp, Ki, Kd;
double integral;
+ double maxIntegral = std::numeric_limits<double>::max();
double derivative;
double previousError;
double processValue;
@@ -69,41 +71,6 @@ namespace armarx
};
typedef boost::shared_ptr<PIDController> PIDControllerPtr;
- class MultiDimPIDController :
- public Logging
- {
- public:
- MultiDimPIDController(float Kp,
- float Ki,
- float Kd,
- double maxControlValue = std::numeric_limits<double>::max(),
- double maxDerivation = std::numeric_limits<double>::max(),
- bool threadSafe = true,
- std::vector<bool> limitless = {});
- void update(const double deltaSec, const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue);
- void update(const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue);
- const Eigen::VectorXf& getControlValue() const;
- void setMaxControlValue(double value);
- void reset();
- // protected:
- float Kp, Ki, Kd;
- double integral;
- double derivative;
- double previousError;
- Eigen::VectorXf processValue;
- Eigen::VectorXf target;
- IceUtil::Time lastUpdateTime;
- Eigen::VectorXf controlValue;
- double maxControlValue;
- double maxDerivation;
- bool firstRun;
- mutable RecursiveMutex mutex;
- bool threadSafe = true;
- std::vector<bool> limitless;
- private:
- ScopedRecursiveLockPtr getLock() const;
- };
- typedef boost::shared_ptr<MultiDimPIDController> MultiDimPIDControllerPtr;
}
diff --git a/source/RobotAPI/libraries/core/Pose.cpp b/source/RobotAPI/libraries/core/Pose.cpp
index f6e8963d3ac67555c690e60df458bcf66595b4cf..cf2ef51c61f126f2e7dd25a79d68937fb91b505d 100644
--- a/source/RobotAPI/libraries/core/Pose.cpp
+++ b/source/RobotAPI/libraries/core/Pose.cpp
@@ -147,7 +147,7 @@ namespace armarx
string Vector3::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen();
+ s << toEigen().format(ArmarXEigenFormat);
return s.str();
}
@@ -247,7 +247,7 @@ namespace armarx
string Quaternion::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen();
+ s << toEigen()/*.format(ArmarXEigenFormat)*/;
return s.str();
}
@@ -328,7 +328,7 @@ namespace armarx
string Pose::output(const Ice::Current& c) const
{
std::stringstream s;
- s << toEigen();
+ s << toEigen()/*.format(ArmarXEigenFormat)*/;
return s.str();
}
diff --git a/source/RobotAPI/libraries/core/Pose.h b/source/RobotAPI/libraries/core/Pose.h
index a3c79e75050b949ff974922122f96e5c1c852ef5..6911f787073c25528b707b632444c638a55e766e 100644
--- a/source/RobotAPI/libraries/core/Pose.h
+++ b/source/RobotAPI/libraries/core/Pose.h
@@ -47,6 +47,9 @@ namespace armarx
const VariantTypeId Pose = Variant::addTypeName("::armarx::PoseBase");
}
+ const Eigen::IOFormat ArmarXEigenFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", "\n", "", "");
+
+
/**
* @brief The Vector2 class
* @ingroup VariantsGrp
diff --git a/source/RobotAPI/libraries/core/RobotPool.cpp b/source/RobotAPI/libraries/core/RobotPool.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d12f6f2b16a0786f9a72231e09a61631b50923c3
--- /dev/null
+++ b/source/RobotAPI/libraries/core/RobotPool.cpp
@@ -0,0 +1,110 @@
+/*
+ * 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
+ */
+#include "RobotPool.h"
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+namespace armarx
+{
+
+ RobotPool::RobotPool(VirtualRobot::RobotPtr robot, size_t defaultSize) :
+ baseRobot(robot)
+ {
+ std::vector<VirtualRobot::RobotPtr> tempVec;
+ for (size_t i = 0; i < defaultSize; ++i)
+ {
+ tempVec.push_back(getRobot());
+ }
+ }
+
+ VirtualRobot::RobotPtr RobotPool::getRobot(int inflation)
+ {
+ ScopedLock lock(mutex);
+ for (auto& r : robots[inflation])
+ {
+ if (r.use_count() == 1)
+ {
+ return r;
+ }
+ }
+ auto newRobot = baseRobot->clone("", VirtualRobot::CollisionCheckerPtr(new VirtualRobot::CollisionChecker()));
+ newRobot->inflateCollisionModel(inflation);
+ ARMARX_CHECK_EQUAL(newRobot.use_count(), 1);
+ ARMARX_DEBUG << "created new robot clone n with inflation " << inflation;
+ robots[inflation].push_back(newRobot);
+ return newRobot;
+ }
+
+ size_t RobotPool::getPoolSize() const
+ {
+ ScopedLock lock(mutex);
+ size_t size = 0;
+ for (auto& pair : robots)
+ {
+ size += pair.second.size();
+ }
+ return size;
+ }
+
+ size_t RobotPool::getRobotsInUseCount() const
+ {
+ ScopedLock lock(mutex);
+ size_t count = 0;
+ for (auto& pair : robots)
+ {
+ for (auto& r : pair.second)
+ {
+ if (r.use_count() > 1)
+ {
+ count++;
+ }
+ }
+ }
+ return count;
+ }
+
+ size_t RobotPool::clean()
+ {
+ ScopedLock lock(mutex);
+ size_t count = 0;
+ for (auto& pair : robots)
+ {
+ std::vector<VirtualRobot::RobotPtr> newList;
+ for (auto& r : pair.second)
+ {
+ if (r.use_count() > 1)
+ {
+ newList.push_back(r);
+ }
+ else
+ {
+ count++;
+ }
+ }
+ pair.second = newList;
+ }
+ return count;
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/RobotPool.h b/source/RobotAPI/libraries/core/RobotPool.h
new file mode 100644
index 0000000000000000000000000000000000000000..23365e7f1edc91b175b6edf622bdd330f3044b3a
--- /dev/null
+++ b/source/RobotAPI/libraries/core/RobotPool.h
@@ -0,0 +1,55 @@
+/*
+ * 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/Robot.h>
+#include <ArmarXCore/core/system/Synchronization.h>
+namespace armarx
+{
+
+ class RobotPool
+ {
+ public:
+ RobotPool(VirtualRobot::RobotPtr robot, size_t defaultSize = 1);
+ /**
+ * @brief getRobot
+ * @param inflation inflation of collision model in mm
+ * @return
+ */
+ VirtualRobot::RobotPtr getRobot(int inflation = 0);
+ size_t getPoolSize() const;
+ size_t getRobotsInUseCount() const;
+ /**
+ * @brief Removes unused robots from the pool.
+ * @return number of cleaned up robots
+ */
+ size_t clean();
+ private:
+ std::map<int, std::vector<VirtualRobot::RobotPtr>> robots;
+ VirtualRobot::RobotPtr baseRobot;
+ mutable Mutex mutex;
+ };
+ typedef std::shared_ptr<RobotPool> RobotPoolPtr;
+}
+
+
diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index a3cf6245cea50b9a1745480fd01d05b59b9e95bc..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;
@@ -211,38 +214,53 @@ namespace armarx
typename timestamp_view::iterator it = dataMap.find(t);
- if (it == dataMap.end() || !it->data.at(dim))
+ if (it == dataMap.end() || dim >= it->data.size() || !it->data.at(dim) || it->data.at(dim)->size() <= derivation)
{
+ // ARMARX_INFO << "interpolating for " << VAROUT(t) << VAROUT(dim);
__fillBaseDataAtTimestamp(t);// interpolates and retrieves
it = dataMap.find(t);
}
+ if (it->data.size() <= dim)
+ {
+ // ARMARX_ERROR << "FAILED!";
+ // ARMARX_INFO << VAROUT(t) << VAROUT(dim) << VAROUT(it->data.size()) << this->output();
+ throw LocalException() << "std::vector ptr is not the correct size!? " << VAROUT(dim) << VAROUT(it->data.size());
+ }
+
if (!it->data.at(dim))
// it->data.at(dim).reset(new Ice::DoubleSeq());
{
throw LocalException() << "std::vector ptr is NULL!?";
}
- std::vector<DoubleSeqPtr>& vec = it->data;
+
+ std::vector<DoubleSeqPtr>& vec = it->data;
+ ARMARX_CHECK_GREATER(vec.size(), dim);
if (derivation != 0 && vec.at(dim)->size() <= derivation)
{
//resize and calculate derivations
- size_t curDeriv = it->data.at(dim)->size();
- it->data.at(dim)->resize(derivation + 1);
+ ARMARX_CHECK_GREATER(vec.size(), dim);
+ ARMARX_CHECK_EXPRESSION(vec.at(dim));
+
+ size_t curDeriv = vec.at(dim)->size();
+ // ARMARX_INFO << VAROUT(curDeriv) << VAROUT(dim);
+ vec.at(dim)->resize(derivation + 1);
while (curDeriv <= derivation)
{
double derivValue = getDiscretDifferentiationForDimAtT(t, dim, curDeriv);
checkValue(curDeriv);
- it->data.at(dim)->at(curDeriv) = derivValue;
+ vec.at(dim)->at(curDeriv) = derivValue;
curDeriv++;
}
}
+ ARMARX_CHECK_GREATER_W_HINT(vec.at(dim)->size(), derivation, VAROUT(t) << VAROUT(dim) << VAROUT(*this));
// std::cout << "dimensions: " <<it->data.size() << " derivations: " << it->data.at(dim)->size() << std::endl;
- double result = it->data.at(dim)->at(derivation);
+ double result = vec.at(dim)->at(derivation);
// checkValue(result);
return result;
}
@@ -261,7 +279,7 @@ namespace armarx
{
if (derivation == 0)
{
- return __calcBaseDataAtTimestamp(t).at(dim)->at(derivation); // interpolates and retrieves
+ return __interpolate(t, dim, derivation);
}
else
{
@@ -321,7 +339,7 @@ namespace armarx
return dimensionNames.at(dim);
}
- Ice::StringSeq Trajectory::getDimensionNames() const
+ const Ice::StringSeq& Trajectory::getDimensionNames() const
{
return dimensionNames;
}
@@ -795,6 +813,7 @@ namespace armarx
std::vector<DoubleSeqPtr> Trajectory::__calcBaseDataAtTimestamp(const double& t) const
{
+ // ARMARX_INFO << "calcBaseDataAtTimestamp for " << t;
// typename timestamp_view::const_iterator it = dataMap.find(t);
// if(it != dataMap.end())
// return it->data;
@@ -805,7 +824,6 @@ namespace armarx
double newValue = __interpolate(t, dimension, 0);
checkValue(newValue);
result.push_back(DoubleSeqPtr(new Ice::DoubleSeq(1, newValue)));
-
}
return result;
@@ -831,20 +849,22 @@ namespace armarx
{
typename timestamp_view::const_iterator it = dataMap.find(t);
- if (it != dataMap.end())
+ if (it != dataMap.end() && it->data.size() == dim())
{
bool foundEmpty = false;
for (size_t i = 0; i < it->data.size(); i++)
{
- if (!it->data.at(i))
+ if (!it->data.at(i) || it->data.at(i)->empty())
{
foundEmpty = true;
+ break;
}
}
if (!foundEmpty)
{
+ // ARMARX_INFO << "Was not empty for " << t;
return it;
}
}
@@ -855,7 +875,7 @@ namespace armarx
dataMap.insert(entry);
it = dataMap.find(t);
- assert(it != dataMap.end());
+ ARMARX_CHECK_EXPRESSION(it != dataMap.end());
// const ordered_view& ordv = dataMap.get<TagOrdered>();
// typename ordered_view::iterator itOrdered = ordv.iterator_to(*it);
it->data = __calcBaseDataAtTimestamp(t);
@@ -1192,7 +1212,7 @@ namespace armarx
{
if (derivation == 0)
{
- getState(t, trajDimension, derivation);
+ return getState(t, trajDimension, derivation);
}
typename timestamp_view::iterator it = dataMap.find(t);
@@ -1252,7 +1272,7 @@ namespace armarx
if (itNext == itPrev)
{
- throw LocalException() << "Interpolation failed: the next data and the previous are missing.\nInfo:\n" << VAROUT(t) << VAROUT(trajDimension) << (getDimensionName(trajDimension)) << VAROUT(size());
+ throw LocalException() << "Interpolation failed: the next data and the previous are missing.\nInfo:\n" << VAROUT(t) << VAROUT(trajDimension) << (getDimensionName(trajDimension)) << " " << VAROUT(size());
}
// double diff = itNext->data[trajDimension]->at(derivation-1) - itPrev->data[trajDimension]->at(derivation-1);
@@ -1452,24 +1472,28 @@ namespace armarx
double next = 0;
// find previous SystemState that exists for that dimension
- while (itPrev != ordView.end() && (itPrev->data.at(dimension) == NULL /*|| itPrev->data[dimension]->size() <= derivation*/))
+ while (itPrev != ordView.end() && (itPrev->data.at(dimension) == NULL || itPrev->data.at(dimension)->size() <= derivation))
{
itPrev--;
}
if (itPrev != ordView.end())
{
+ // ARMARX_INFO << "Found prev state at " << itPrev->timestamp;
+ ARMARX_CHECK_NOT_EQUAL_W_HINT(t, itPrev->timestamp, VAROUT(t) << VAROUT(itPrev->timestamp) << VAROUT(*this));
previous = getState(itPrev->timestamp, dimension, derivation);
}
// find next SystemState that exists for that dimension
- while (itNext != ordView.end() && (itNext->data.at(dimension) == NULL /*|| itNext->data[dimension]->size() <= derivation*/))
+ while (itNext != ordView.end() && (!itNext->data.at(dimension) || itNext->data.at(dimension)->size() <= derivation))
{
itNext++;
}
if (itNext != ordView.end())
{
+ // ARMARX_INFO << "Found next state at " << itNext->timestamp;
+ ARMARX_CHECK_NOT_EQUAL_W_HINT(t, itNext->timestamp, VAROUT(t) << VAROUT(itNext->timestamp));
next = getState(itNext->timestamp, dimension, derivation);
}
@@ -1482,10 +1506,12 @@ namespace armarx
if (itNext == ordView.end())
{
+ // ARMARX_INFO << "Extrapolating to the right from " << itPrev->timestamp;
return getState(itPrev->timestamp, dimension, derivation) + getState(itPrev->timestamp, dimension, derivation + 1) * (t - itPrev->timestamp);
}
else if (itPrev == ordView.end())
{
+ // ARMARX_INFO << "Extrapolating to the left from " << itNext->timestamp;
return getState(itNext->timestamp, dimension, derivation) - getState(itNext->timestamp, dimension, derivation + 1) * (itNext->timestamp - t);
}
else
@@ -1596,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();
}
@@ -1623,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)
{
@@ -1636,7 +1671,7 @@ namespace armarx
currentTimestamp += stepSize;
i++;
}
-
+ ARMARX_CHECK_EQUAL_W_HINT(result.size(), size, VAROUT(startTime) << VAROUT(endTime) << VAROUT(stepSize));
return result;
}
@@ -1692,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;
+ }
+
@@ -1867,11 +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++)
{
- //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 15cef1251f47b3f1c7d782b4dc54c1aea4e96217..5f7613bdca048ee60f3efdcc43b53e7d9f79ca62 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -162,7 +162,7 @@ namespace armarx
stream << rhs.timestamp << ": ";
for (size_t d = 0; d < rhs.data.size(); ++d)
{
- stream << (rhs.data[d]->size() > 0 ? to_string(rhs.data[d]->at(0)) : std::string("-"));
+ stream << (rhs.data[d] && rhs.data[d]->size() > 0 ? to_string(rhs.data[d]->at(0)) : std::string("-"));
if (d <= rhs.data.size() - 1)
{
stream << ", ";
@@ -296,7 +296,7 @@ namespace armarx
std::vector<Ice::DoubleSeq > getAllStates(double t, int maxDeriv = 1);
Ice::DoubleSeq getDerivations(double t, size_t dimension, size_t derivations) const;
std::string getDimensionName(size_t dim) const;
- Ice::StringSeq getDimensionNames() const;
+ const Ice::StringSeq& getDimensionNames() const;
TrajDataContainer& data();
@@ -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/TrajectoryController.cpp b/source/RobotAPI/libraries/core/TrajectoryController.cpp
index 063bef2f10e5ccfb62fdf15bdcb3f6c8d42bb783..ef74dd1fc47b1f67dbc44bb97f87f5dc5346a39c 100644
--- a/source/RobotAPI/libraries/core/TrajectoryController.cpp
+++ b/source/RobotAPI/libraries/core/TrajectoryController.cpp
@@ -24,10 +24,11 @@
#include "TrajectoryController.h"
#include <RobotAPI/libraries/core/math/LinearizeAngularTrajectory.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
+
namespace armarx
{
- TrajectoryController::TrajectoryController(const TrajectoryPtr& traj, float kp, float ki, float kd, bool threadSafe) :
+ TrajectoryController::TrajectoryController(const TrajectoryPtr& traj, float kp, float ki, float kd, bool threadSafe, float maxIntegral) :
traj(traj)
{
std::vector<bool> limitless;
@@ -36,6 +37,8 @@ namespace armarx
limitless.push_back(ls.enabled);
}
pid.reset(new MultiDimPIDController(kp, ki, kd, std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), threadSafe, limitless));
+ pid->maxIntegral = maxIntegral;
+ pid->preallocate(traj->dim());
ARMARX_CHECK_EXPRESSION(traj);
currentTimestamp = traj->begin()->getTimestamp();
//for (size_t i = 0; i < traj->dim(); i++)
@@ -47,26 +50,40 @@ namespace armarx
veloctities.resize(traj->dim(), 1);
}
- Eigen::VectorXf TrajectoryController::update(double deltaT, const Eigen::VectorXf currentPosition)
+ const Eigen::VectorXf& TrajectoryController::update(double deltaT, const Eigen::VectorXf& currentPosition)
{
ARMARX_CHECK_EXPRESSION(pid);
ARMARX_CHECK_EXPRESSION(traj);
+ ARMARX_CHECK_EXPRESSION(traj->size() > 0);
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(currentPosition.rows()), traj->dim());
- int dim = traj->dim();
+ size_t dim = traj->dim();
currentTimestamp = currentTimestamp + deltaT;
-
+ const Trajectory& traj = *this->traj;
if (currentTimestamp < 0.0)
{
currentTimestamp = 0.0;
}
+ if (currentTimestamp <= traj.rbegin()->getTimestamp())
+ {
+ for (size_t i = 0; i < dim; ++i)
+ {
- for (int i = 0; i < dim; ++i)
+ positions(i) = traj.getState(currentTimestamp, i, 0);
+ veloctities(i) = (std::signbit(deltaT) ? -1.0 : 1.0) * traj.getState(currentTimestamp, i, 1);
+ //pids.at(i)->update(std::abs(deltaT), currentPosition(i), positions(i));
+ //veloctities(i) += pids.at(i)->getControlValue();
+ }
+ }
+ else // hold position in the end
{
- positions(i) = traj->getState(currentTimestamp, i, 0);
- veloctities(i) = (std::signbit(deltaT) ? -1.0 : 1.0) * traj->getState(currentTimestamp, i, 1);
- //pids.at(i)->update(std::abs(deltaT), currentPosition(i), positions(i));
- //veloctities(i) += pids.at(i)->getControlValue();
+ for (size_t i = 0; i < dim; ++i)
+ {
+ positions(i) = traj.rbegin()->getPosition(i);
+ veloctities(i) = 0;
+ }
}
+ currentError = positions - currentPosition;
+
pid->update(std::abs(deltaT), currentPosition, positions);
veloctities += pid->getControlValue();
return veloctities;
@@ -154,5 +171,15 @@ namespace armarx
}
}
+ const Eigen::VectorXf& TrajectoryController::getCurrentError() const
+ {
+ return currentError;
+ }
+
+ const Eigen::VectorXf& TrajectoryController::getPositions() const
+ {
+ return positions;
+ }
+
} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/TrajectoryController.h b/source/RobotAPI/libraries/core/TrajectoryController.h
index a12d592f4f9fdefcf7e96bc6fc0967c09f95ac24..2b4c44983357322de378f71ee753711a349b02f6 100644
--- a/source/RobotAPI/libraries/core/TrajectoryController.h
+++ b/source/RobotAPI/libraries/core/TrajectoryController.h
@@ -32,8 +32,8 @@ namespace armarx
class TrajectoryController
{
public:
- TrajectoryController(const TrajectoryPtr& traj, float kp, float ki = 0.0f, float kd = 0.0f, bool threadSafe = true);
- Eigen::VectorXf update(double deltaT, const Eigen::VectorXf currentPosition);
+ TrajectoryController(const TrajectoryPtr& traj, float kp, float ki = 0.0f, float kd = 0.0f, bool threadSafe = true, float maxIntegral = std::numeric_limits<float>::max());
+ const Eigen::VectorXf& update(double deltaT, const Eigen::VectorXf& currentPosition);
//const MultiDimPIDControllerPtr& getPid() const;
//void setPid(const MultiDimPIDControllerPtr& value);
@@ -44,6 +44,10 @@ namespace armarx
static void UnfoldLimitlessJointPositions(TrajectoryPtr traj);
static void FoldLimitlessJointPositions(TrajectoryPtr traj);
+ const Eigen::VectorXf& getCurrentError() const;
+
+ const Eigen::VectorXf& getPositions() const;
+
protected:
TrajectoryPtr traj;
MultiDimPIDControllerPtr pid;
@@ -51,6 +55,7 @@ namespace armarx
double currentTimestamp;
Eigen::VectorXf positions;
Eigen::VectorXf veloctities;
+ Eigen::VectorXf currentError;
};
typedef std::shared_ptr<TrajectoryController> TrajectoryControllerPtr;
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/remoterobot/RobotStateObserver.cpp b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
index 96f3271324461e52a93cef33b81fcc814da03b2c..24e7f8467554d87c8797809169cfb701e80bc7f8 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
@@ -22,6 +22,7 @@
#include "RobotStateObserver.h"
#include <RobotAPI/interface/core/RobotState.h>
+#include <ArmarXCore/core/util/algorithm.h>
#include <ArmarXCore/observers/checks/ConditionCheckEquals.h>
#include <ArmarXCore/observers/checks/ConditionCheckInRange.h>
#include <ArmarXCore/observers/checks/ConditionCheckLarger.h>
@@ -35,6 +36,7 @@
#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/VirtualRobot.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <boost/algorithm/string/trim.hpp>
@@ -206,53 +208,38 @@ void RobotStateObserver::updateNodeVelocities(const NameValueMap& jointVel, long
tcpPoses[tcpName] = new FramedPose(currentPose, GlobalFrame, "");
}
- IceUtil::Time currentTimestamp = IceUtil::Time::microSeconds(timestampMicroSeconds);
- double tDelta = 0.0f;
- if (lastVelocityUpdate.toMicroSeconds() > 0)
- {
- tDelta = (lastVelocityUpdate - currentTimestamp).toMilliSecondsDouble();
- }
-
- for (NameValueMap::iterator it = tempJointAngles.begin(); it != tempJointAngles.end(); it++)
- {
- NameValueMap::const_iterator itSrc = jointVel.find(it->first);
-
- if (itSrc != jointVel.end())
- {
- it->second += itSrc->second * tDelta;
- }
- }
velocityReportRobot->setJointValues(tempJointAngles);
velocityReportRobot->setGlobalPose(robot->getGlobalPose());
- // ARMARX_INFO << deactivateSpam(1) << "duration: " << (IceUtil::Time::now()-start).toMicroSecondsDouble();
- // start = IceUtil::Time::now();
+
Eigen::Matrix4f mat;
Eigen::Vector3f rpy;
- for (unsigned int i = 0; i < nodesToReport.size(); i++)
+ auto keys = armarx::getMapKeys(jointVel);
+ Eigen::VectorXf jointVelocities(jointVel.size());
+ auto rootFrameName = velocityReportRobot->getRootNode()->getName();
+ RobotNodeSetPtr rns = RobotNodeSet::createRobotNodeSet(velocityReportRobot, "All_Nodes", keys, rootFrameName);
+ for (size_t i = 0; i < rns->getSize(); ++i)
{
- RobotNodePtr node = velocityReportRobot->getRobotNode(nodesToReport.at(i).first->getName());
- const std::string& tcpName = node->getName();
- const Eigen::Matrix4f& currentPose = node->getGlobalPose();
-
+ jointVelocities(i) = jointVel.at(rns->getNode(i)->getName());
+ }
+ DifferentialIKPtr j(new DifferentialIK(rns));
- FramedPosePtr lastPose = FramedPosePtr::dynamicCast(tcpPoses[tcpName]);
- tcpTranslationVelocities[tcpName] = new FramedDirection((currentPose.block(0, 3, 3, 1) - lastPose->toEigen().block(0, 3, 3, 1)) / tDelta, GlobalFrame, "");
- FramedDirectionPtr::dynamicCast(tcpTranslationVelocities[tcpName])->changeFrame(velocityReportRobot, nodesToReport.at(i).second);
- mat = currentPose * lastPose->toEigen().inverse();
+ auto robotName = velocityReportRobot->getName();
+ for (unsigned int i = 0; i < nodesToReport.size(); i++)
+ {
+ RobotNodePtr node = velocityReportRobot->getRobotNode(nodesToReport.at(i).first->getName());
+ Eigen::MatrixXf jac = j->getJacobianMatrix(node, IKSolver::All);
+ Eigen::VectorXf nodeVel = jac * jointVelocities;
- VirtualRobot::MathTools::eigen4f2rpy(mat, rpy);
+ const std::string tcpName = node->getName();
+ tcpTranslationVelocities[tcpName] = new FramedDirection(nodeVel.block<3, 1>(0, 0), rootFrameName, robotName);
+ tcpOrientationVelocities[tcpName] = new FramedDirection(nodeVel.block<3, 1>(3, 0), rootFrameName, robotName);
- tcpOrientationVelocities[tcpName] = new FramedDirection(rpy / tDelta, GlobalFrame, "");
- FramedDirectionPtr::dynamicCast(tcpOrientationVelocities[tcpName])->changeFrame(velocityReportRobot, nodesToReport.at(i).second);
}
-
- // ARMARX_INFO << deactivateSpam(1) << "duration2: " << (IceUtil::Time::now()-start).toMicroSecondsDouble();
- // ARMARX_INFO << deactivateSpam(5) << "TCP Pose Vel Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
updateVelocityDatafields(tcpTranslationVelocities, tcpOrientationVelocities);
}
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/CartesianVelocityControllerTest.cpp b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
index 0e889f0c0efec591d6e1caf6a1b2d20fe0ef6f11..f50dedc362145b2ef1aeb812f98233ba0f6da8d4 100644
--- a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
+++ b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerTest.cpp
@@ -66,7 +66,7 @@ BOOST_AUTO_TEST_CASE(test1)
jointVel << 1, 1, 1, 1, 1, 1, 1, 1;
Eigen::VectorXf targetTcpVel(6);
- targetTcpVel << 1, 0, 0, 0, 0, 0;
+ targetTcpVel << 0, 0, 0, 0, 0, 0;
ARMARX_IMPORTANT << rns->getJointValues();
Eigen::VectorXf vel = h->calculate(targetTcpVel, jointVel, VirtualRobot::IKSolver::CartesianSelection::All);
diff --git a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp
index 487018abde075ae58f1eca8696b84a76685e002a..5ffd4e0aed928a3e9292cabbb25087cf91758b6f 100644
--- a/source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp
+++ b/source/RobotAPI/libraries/core/test/CartesianVelocityControllerWithRampTest.cpp
@@ -50,7 +50,7 @@ BOOST_AUTO_TEST_CASE(CartesianVelocityRampTest)
for (size_t i = 0; i < 30; i++)
{
state = c.update(target, dt);
- ARMARX_IMPORTANT << "state:" << state;
+ ARMARX_IMPORTANT << "state:" << state.transpose();
}
}
@@ -77,20 +77,20 @@ BOOST_AUTO_TEST_CASE(test1)
Eigen::VectorXf currentjointVel(8);
- currentjointVel << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
+ currentjointVel << 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0;
Eigen::VectorXf targetTcpVel(6);
- targetTcpVel << 20, 0, 0, 0, 0, 0;
+ targetTcpVel << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
- CartesianVelocityControllerWithRampPtr h(new CartesianVelocityControllerWithRamp(rns, currentjointVel, VirtualRobot::IKSolver::CartesianSelection::All, 100, 1, 1));
+ CartesianVelocityControllerWithRampPtr h(new CartesianVelocityControllerWithRamp(rns, currentjointVel, VirtualRobot::IKSolver::CartesianSelection::All, 100, 1, 0.1));
Eigen::MatrixXf jacobi = h->controller.ik->getJacobianMatrix(rns->getTCP(), VirtualRobot::IKSolver::CartesianSelection::All);
ARMARX_IMPORTANT << "Initial TCP Vel: " << (jacobi * currentjointVel).transpose();
- float dt = 0.1f;
+ float dt = 0.01f;
- for (size_t n = 0; n < 10; n++)
+ for (size_t n = 0; n < 300; n++)
{
//ARMARX_IMPORTANT << rns->getJointValues();
Eigen::VectorXf vel = h->calculate(targetTcpVel, 2, dt);
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/RobotTest.cpp b/source/RobotAPI/libraries/core/test/RobotTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c34a4d38a05e660e016bc768612dd12956221f79
--- /dev/null
+++ b/source/RobotAPI/libraries/core/test/RobotTest.cpp
@@ -0,0 +1,75 @@
+/*
+ * 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
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::RobotTest::Test
+#define ARMARX_BOOST_TEST
+#include <RobotAPI/Test.h>
+#include <ArmarXCore/core/test/IceTestHelper.h>
+#include <ArmarXCore/util/json/JSONObject.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include "../RobotPool.h"
+
+using namespace armarx;
+
+
+BOOST_AUTO_TEST_CASE(RobotPoolTest)
+{
+ const std::string project = "RobotAPI";
+ armarx::CMakePackageFinder finder(project);
+
+ ARMARX_CHECK_EXPRESSION_W_HINT(finder.packageFound(), project);
+
+
+ armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
+
+ std::string fn = "RobotAPI/robots/Armar3/ArmarIII.xml";
+ ArmarXDataPath::getAbsolutePath(fn, fn);
+ std::string robotFilename = fn;
+ VirtualRobot::RobotPtr robot = VirtualRobot::RobotIO::loadRobot(robotFilename, VirtualRobot::RobotIO::eCollisionModel);
+ RobotPool pool(robot, 2);
+ {
+ BOOST_CHECK_EQUAL(pool.getPoolSize(), 2);
+ auto clone = pool.getRobot();
+ BOOST_CHECK_EQUAL(pool.getPoolSize(), 2);
+ BOOST_CHECK_EQUAL(pool.getRobotsInUseCount(), 1);
+ {
+ auto clone2 = pool.getRobot();
+ BOOST_CHECK_EQUAL(pool.getPoolSize(), 2);
+ BOOST_CHECK_EQUAL(pool.getRobotsInUseCount(), 2);
+ }
+ BOOST_CHECK_EQUAL(pool.getPoolSize(), 2);
+ BOOST_CHECK_EQUAL(pool.getRobotsInUseCount(), 1);
+ auto clone2 = pool.getRobot();
+ BOOST_CHECK_EQUAL(pool.getPoolSize(), 2);
+ BOOST_CHECK_EQUAL(pool.getRobotsInUseCount(), 2);
+ }
+ BOOST_CHECK_EQUAL(pool.getRobotsInUseCount(), 0);
+ BOOST_CHECK_EQUAL(pool.clean(), 2);
+ BOOST_CHECK_EQUAL(pool.getPoolSize(), 0);
+
+}
+
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>
+