diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index fac11b677edbf7fc26cd2538e453981601e94f13..9d9342ab21b51d11fd188a70c5e4cf6066afff24 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -43,10 +43,18 @@
<Proxy include="RobotAPI/interface/units/PlatformUnitInterface.h"
humanName="Platform Unit (obstacle avoiding)"
typeName="PlatformUnitInterfacePrx"
- memberName="obstacleAvoidingPlatformUnit"
- getterName="getObstacleAvoidingPlatformUnit"
- propertyName="ObstacleAvoidingPlatformUnitName"
+ memberName="platformUnit1"
+ getterName="getPlatformUnit1"
+ propertyName="PlatformUnitName1"
propertyDefaultValue="ObstacleAvoidingPlatformUnit"
+ propertyIsOptional="true" />
+ <Proxy include="RobotAPI/interface/units/PlatformUnitInterface.h"
+ humanName="Platform Unit (obstacle aware)"
+ typeName="PlatformUnitInterfacePrx"
+ memberName="platformUnit2"
+ getterName="getPlatformUnit2"
+ propertyName="PlatformUnitName2"
+ propertyDefaultValue="ObstacleAwarePlatformUnit"
propertyIsOptional="true" />
<Proxy include="RobotAPI/interface/observers/PlatformUnitObserverInterface.h"
humanName="Platform Unit Observer"
diff --git a/scenarios/dasd/config/DynamicObstacleManager.cfg b/scenarios/dasd/config/DynamicObstacleManager.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..0eabec6c1f57706ae48e25fe4b2f92f67c85c8a5
--- /dev/null
+++ b/scenarios/dasd/config/DynamicObstacleManager.cfg
@@ -0,0 +1,327 @@
+# ==================================================================
+# DynamicObstacleManager properties
+# ==================================================================
+
+# 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: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - 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: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# 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: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# 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: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.DynamicObstacleManager.AllowInRobotSpawning: Allow obstacles to spawn inside the robot
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DynamicObstacleManager.AllowInRobotSpawning = false
+
+
+# ArmarX.DynamicObstacleManager.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.DynamicObstacleManager.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DynamicObstacleManager.EnableProfiling = false
+
+
+# ArmarX.DynamicObstacleManager.EnableRemove: Delete Obstacles when value < 0
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+ArmarX.DynamicObstacleManager.EnableRemove = true
+
+
+# ArmarX.DynamicObstacleManager.LineSecurityMargin: Security margin of line obstacles
+# Attributes:
+# - Default: 400
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.LineSecurityMargin = 250
+
+
+# ArmarX.DynamicObstacleManager.LineThickness: The thickness of line obstacles
+# Attributes:
+# - Default: 200
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.LineThickness = 200
+
+
+# ArmarX.DynamicObstacleManager.MaxLengthOfLines: Maximum length of lines in mm
+# Attributes:
+# - Default: 10000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.MaxLengthOfLines = 10000
+
+
+# ArmarX.DynamicObstacleManager.MaxObstacleSize: The maximal obstacle size in mm.
+# Attributes:
+# - Default: 600
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.MaxObstacleSize = 400
+
+
+# ArmarX.DynamicObstacleManager.MaxObstacleValue: Maximum value for the obstacles
+# Attributes:
+# - Default: 5000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.MaxObstacleValue = 5000
+
+
+# ArmarX.DynamicObstacleManager.MinLengthOfLines: Minimum length of lines in mm
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.MinLengthOfLines = 200
+
+
+# ArmarX.DynamicObstacleManager.MinObstacleSize: The minimal obstacle size in mm.
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.MinObstacleSize = 5
+
+
+# ArmarX.DynamicObstacleManager.MinObstacleValueForAccepting: Minimum value for the obstacles to get accepted
+# Attributes:
+# - Default: 1000
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.MinObstacleValueForAccepting = 50
+
+
+# ArmarX.DynamicObstacleManager.MinSampleRatioPerEllipsis: Minimum percentage of samples which have to be in an elllipsis to be considered as known obsacle
+# Attributes:
+# - Default: 0.699999988
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.MinSampleRatioPerEllipsis = 0.699999988
+
+
+# ArmarX.DynamicObstacleManager.MinSampleRatioPerLineSegment: Minimum percentage of samples which have to be in an elllipsis to be considered as known obsacle
+# Attributes:
+# - Default: 0.899999976
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.MinSampleRatioPerLineSegment = 0.5
+
+
+# ArmarX.DynamicObstacleManager.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+ArmarX.DynamicObstacleManager.MinimumLoggingLevel = Debug
+
+
+# ArmarX.DynamicObstacleManager.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.ObjectName = ""
+
+
+# ArmarX.DynamicObstacleManager.ObstacleAvoidanceName: The name of the used obstacle avoidance interface
+# Attributes:
+# - Default: PlatformObstacleAvoidance
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DynamicObstacleManager.ObstacleAvoidanceName = PlatformObstacleAvoidance
+
+
+# ArmarX.DynamicObstacleManager.ObstacleSecurityMargin: Security margin of ellipsis obstacles
+# Attributes:
+# - Default: 500
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.ObstacleSecurityMargin = 300
+
+
+# ArmarX.DynamicObstacleManager.OnlyVisualizeObstacles: Connection to obstacle avoidance
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DynamicObstacleManager.OnlyVisualizeObstacles = false
+
+
+# ArmarX.DynamicObstacleManager.UpdateInterval: The interval to check the obstacles
+# Attributes:
+# - Default: 500
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.DynamicObstacleManager.UpdateInterval = 350
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# 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: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - 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: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/dasd/config/ObstacleAwarePlatformUnit.cfg b/scenarios/dasd/config/ObstacleAwarePlatformUnit.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..c397d978b79078ebfd9908eb5bfd7c9a3e103c59
--- /dev/null
+++ b/scenarios/dasd/config/ObstacleAwarePlatformUnit.cfg
@@ -0,0 +1,332 @@
+# ==================================================================
+# ObstacleAwarePlatformUnit properties
+# ==================================================================
+
+# 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: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - 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: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# 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: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# 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: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.ObstacleAwarePlatformUnit.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.ObstacleAwarePlatformUnit.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.ObstacleAwarePlatformUnit.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObstacleAwarePlatformUnit.EnableProfiling = false
+
+
+# ArmarX.ObstacleAwarePlatformUnit.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+ArmarX.ObstacleAwarePlatformUnit.MinimumLoggingLevel = Debug
+
+
+# ArmarX.ObstacleAwarePlatformUnit.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.ObjectName = ""
+
+
+# ArmarX.ObstacleAwarePlatformUnit.PlatformName: Name of the platform (will publish values on PlatformName + 'State')
+# Attributes:
+# - Default: Platform
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ObstacleAwarePlatformUnit.PlatformName = Armar6PlatformUnit
+
+
+# ArmarX.ObstacleAwarePlatformUnit.RemoteStateComponentName: Name of the robot state component
+# Attributes:
+# - Default: RobotStateComponent
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.RemoteStateComponentName = RobotStateComponent
+
+
+# ArmarX.ObstacleAwarePlatformUnit.cmp.DynamicObstacleManager: Ice object name of the `DynamicObstacleManager` component.
+# Attributes:
+# - Default: ObstacleAvoidingPlatformUnit
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ObstacleAwarePlatformUnit.cmp.DynamicObstacleManager = DynamicObstacleManager
+
+
+# ArmarX.ObstacleAwarePlatformUnit.cmp.PlatformUnit: Ice object name of the `PlatformUnit` component.
+# Attributes:
+# - Default: Platform
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.ObstacleAwarePlatformUnit.cmp.PlatformUnit = Armar6PlatformUnit
+
+
+# ArmarX.ObstacleAwarePlatformUnit.cmp.RobotStateComponent: Ice object name of the `RobotStateComponent` component.
+# Attributes:
+# - Default: RobotStateComponent
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.cmp.RobotStateComponent = RobotStateComponent
+
+
+# ArmarX.ObstacleAwarePlatformUnit.control.pos.kp:
+# Attributes:
+# - Default: 3.5
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.control.pos.kp = 3.5
+
+
+# ArmarX.ObstacleAwarePlatformUnit.control.pose.cycle_time: Control loop cycle time.
+# Attributes:
+# - Default: 10 ms
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.control.pose.cycle_time = 10 ms
+
+
+# ArmarX.ObstacleAwarePlatformUnit.control.rot.kd:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.control.rot.kd = 0
+
+
+# ArmarX.ObstacleAwarePlatformUnit.control.rot.ki:
+# Attributes:
+# - Default: 9.00000014e-05
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.control.rot.ki = 9.00000014e-05
+
+
+# ArmarX.ObstacleAwarePlatformUnit.control.rot.kp:
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.control.rot.kp = 1
+
+
+# ArmarX.ObstacleAwarePlatformUnit.doa.agent_safety_margin:
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.doa.agent_safety_margin = 0
+
+
+# ArmarX.ObstacleAwarePlatformUnit.min_vel_general: Velocity in [mm/s] the robot should at least set on general
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.min_vel_general = 100
+
+
+# ArmarX.ObstacleAwarePlatformUnit.min_vel_near_target: Velocity in [mm/s] the robot should at least set when near the target
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.min_vel_near_target = 50
+
+
+# ArmarX.ObstacleAwarePlatformUnit.pos_near_threshold: Distance in [mm] after which the robot is considered to be near the target for min velocity, smoothing, etc.
+# Attributes:
+# - Default: 250
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.pos_near_threshold = 250
+
+
+# ArmarX.ObstacleAwarePlatformUnit.tpc.pub.GlobalRobotPoseLocalization: Name of the `GlobalRobotPoseLocalization` topic to publish data to.
+# Attributes:
+# - Default: GlobalRobotPoseLocalization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.tpc.pub.GlobalRobotPoseLocalization = GlobalRobotPoseLocalization
+
+
+# ArmarX.ObstacleAwarePlatformUnit.tpc.pub.Odometry: Name of the `Odometry` topic to publish data to.
+# Attributes:
+# - Default: Odometry
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObstacleAwarePlatformUnit.tpc.pub.Odometry = Odometry
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# 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: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - 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: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
diff --git a/scenarios/dasd/config/global.cfg b/scenarios/dasd/config/global.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..897f1c82a0578538e7b463f4246587ba134e24de
--- /dev/null
+++ b/scenarios/dasd/config/global.cfg
@@ -0,0 +1,4 @@
+# ==================================================================
+# Global Config from Scenario dasd
+# ==================================================================
+
diff --git a/scenarios/dasd/dasd.scx b/scenarios/dasd/dasd.scx
new file mode 100644
index 0000000000000000000000000000000000000000..b12d5a0c9d6feb2eb4f19076d28212d9294eff02
--- /dev/null
+++ b/scenarios/dasd/dasd.scx
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8"?>
+<scenario name="dasd" creation="2021-06-07.18:00:09" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
+ <application name="ObstacleAwarePlatformUnit" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="DynamicObstacleManager" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+</scenario>
+
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
index e712e30ccb2978fdc7caa0210b919773b7dce46b..41fc3e85f7a6a7250c47fe58c0730973a16480be 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
@@ -22,6 +22,8 @@
#include "DynamicObstacleManager.h"
+#include <VirtualRobot/MathTools.h>
+
// STD/STL
#include <string>
#include <vector>
@@ -118,6 +120,7 @@ namespace armarx
void DynamicObstacleManager::add_decayable_line_segment(const Eigen::Vector2f& line_start, const Eigen::Vector2f& line_end, const Ice::Current&)
{
+ TIMING_START(add_decayable_line_segment);
const Eigen::Vector2f difference_vec = line_end - line_start;
const float length = difference_vec.norm();
const Eigen::Vector2f center_vec = line_start + 0.5 * difference_vec;
@@ -136,10 +139,10 @@ namespace armarx
return;
}
-
{
std::shared_lock<std::shared_mutex> l(m_managed_obstacles_mutex);
+ TIMING_START(add_decayable_line_segment_mutex);
// First check own obstacles
for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
{
@@ -155,13 +158,15 @@ namespace armarx
managed_obstacle->line_matches.push_back(line_start);
managed_obstacle->line_matches.push_back(line_end);
managed_obstacle->m_last_matched = IceUtil::Time::now();
+ TIMING_END(add_decayable_line_segment_mutex);
return;
}
}
+ TIMING_END(add_decayable_line_segment_mutex);
}
// Second check the obstacles from the obstacle avoidance
- for (const auto& published_obstacle : m_obstacle_detection->getObstacles())
+ /*for (const auto& published_obstacle : m_obstacle_detection->getObstacles())
{
float coverage = ManagedObstacle::line_segment_ellipsis_coverage(
{published_obstacle.posX, published_obstacle.posY}, published_obstacle.axisLengthX, published_obstacle.axisLengthY, published_obstacle.yaw,
@@ -172,7 +177,7 @@ namespace armarx
ARMARX_DEBUG << "Found the obstacle in the static obstacle list! Matching name: " << published_obstacle.name;
return;
}
- }
+ }*/
ARMARX_DEBUG << " No matching obstacle found. Create new obstacle and add to list";
ManagedObstaclePtr new_managed_obstacle(new ManagedObstacle());
@@ -198,6 +203,15 @@ namespace armarx
std::lock_guard<std::shared_mutex> l(m_managed_obstacles_mutex);
m_managed_obstacles.push_back(new_managed_obstacle);
}
+ TIMING_END(add_decayable_line_segment);
+ }
+
+ void DynamicObstacleManager::add_decayable_line_segments(const dynamicobstaclemanager::LineSegments& lines, const Ice::Current& c)
+ {
+ for (const auto& line : lines)
+ {
+ add_decayable_line_segment(line.lineStart, line.lineEnd, c);
+ }
}
@@ -218,7 +232,7 @@ namespace armarx
std::lock_guard l(m_managed_obstacles_mutex);
ARMARX_DEBUG << "Remove Obstacle " << name << " from obstacle map and from obstacledetection";
- for (ManagedObstaclePtr& managed_obstacle : m_managed_obstacles)
+ for (ManagedObstaclePtr managed_obstacle : m_managed_obstacles)
{
if (managed_obstacle->m_obstacle.name == name)
{
@@ -240,6 +254,36 @@ namespace armarx
}
+ float
+ DynamicObstacleManager::distanceToObstacle(const Eigen::Vector2f& agentPosition, float safetyRadius, const Eigen::Vector2f& goal, const Ice::Current&)
+ {
+ std::shared_lock<std::shared_mutex> l{m_managed_obstacles_mutex};
+
+ const float sample_step = 5; // in [mm], sample step size towards goal.
+ const float distance_to_goal = (goal - agentPosition).norm() + safetyRadius;
+ float current_distance = safetyRadius;
+
+ while (current_distance < distance_to_goal)
+ {
+ for (const auto man_obstacle : m_managed_obstacles)
+ {
+ Eigen::Vector2f sample = agentPosition + ((goal - agentPosition).normalized() * current_distance);
+ obstacledetection::Obstacle obstacle = man_obstacle->m_obstacle;
+ Eigen::Vector2f obstacle_origin{obstacle.posX, obstacle.posY};
+
+ if (ManagedObstacle::point_ellipsis_coverage(obstacle_origin, obstacle.axisLengthX, obstacle.axisLengthY, obstacle.yaw, sample))
+ {
+ return current_distance - safetyRadius;
+ }
+ }
+
+ current_distance += sample_step;
+ }
+
+ return std::numeric_limits<float>::infinity();
+ }
+
+
void DynamicObstacleManager::directly_update_obstacle(const std::string& name, const Eigen::Vector2f& obs_pos, float e_rx, float e_ry, float e_yaw, const Ice::Current&)
{
obstacledetection::Obstacle new_unmanaged_obstacle;
@@ -283,7 +327,6 @@ namespace armarx
}
}
-
// Update obstacle avoidance
int checked_obstacles = 0;
bool updated = false;
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
index 1629b7efe3102dccf85521ac0fac6379e09225cc..5f2d8065a307706ea16789930c12f5165d6044eb 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.h
@@ -69,10 +69,12 @@ namespace armarx
void add_decayable_obstacle(const Eigen::Vector2f&, float, float, float, const Ice::Current& = Ice::Current()) override;
void add_decayable_line_segment(const Eigen::Vector2f&, const Eigen::Vector2f&, const Ice::Current& = Ice::Current()) override;
+ void add_decayable_line_segments(const dynamicobstaclemanager::LineSegments& lines, const Ice::Current& = Ice::Current()) override;
void remove_all_decayable_obstacles(const Ice::Current& = Ice::Current()) override;
void directly_update_obstacle(const std::string& name, const Eigen::Vector2f&, float, float, float, const Ice::Current& = Ice::Current()) override;
void remove_obstacle(const std::string& name, const Ice::Current& = Ice::Current()) override;
void wait_unitl_obstacles_are_ready(const Ice::Current& = Ice::Current()) override;
+ float distanceToObstacle(const Eigen::Vector2f& agentPosition, float safetyRadius, const Eigen::Vector2f& goal, const Ice::Current& = Ice::Current()) override;
protected:
void onInitComponent() override;
diff --git a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
index fe21cc4e46558b3a70102592ee77ef56715421f8..f718a45434ec538fbd5d2541fb9413568f3e79f0 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/ManagedObstacle.cpp
@@ -94,10 +94,25 @@ namespace armarx
float ManagedObstacle::line_segment_ellipsis_coverage(const Eigen::Vector2f e_origin, const float e_rx, const float e_ry, const float e_yaw, const Eigen::Vector2f line_seg_start, const Eigen::Vector2f line_seg_end)
{
// Again we discretize the line into n points and we check the coverage of those points
- const unsigned int SAMPLES = 40;
const Eigen::Vector2f difference_vec = line_seg_end - line_seg_start;
const Eigen::Vector2f difference_vec_normed = difference_vec.normalized();
const float distance = difference_vec.norm();
+ const unsigned int SAMPLES = std::max(distance / 10.0, 40.0);
+
+ const Vector2f difference_start_origin = e_origin - line_seg_start;
+ const Vector2f difference_end_origin = e_origin - line_seg_end;
+
+ if (difference_start_origin.norm() > std::max(e_rx, e_ry) && distance < std::max(e_rx, e_ry))
+ {
+ return 0.0;
+ }
+
+ if (difference_end_origin.norm() > std::max(e_rx, e_ry) && distance < std::max(e_rx, e_ry))
+ {
+ return 0.0;
+ }
+
+
const float step_size = distance / SAMPLES;
const Eigen::Vector2f dir = difference_vec_normed * step_size;
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index 51d1930b83753c195ff7558154092ea025466e4c..3fa53590170e0ff9d288f7f130adae9814ee7446 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -75,6 +75,7 @@ set(LIB_FILES
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
add_subdirectory(ObstacleAvoidingPlatformUnit)
+add_subdirectory(ObstacleAwarePlatformUnit)
add_subdirectory(relays)
add_subdirectory(RobotUnit)
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..524b650bf5a3767a92a457ab97b26ffcdd903a40
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt
@@ -0,0 +1,10 @@
+armarx_add_component(
+ COMPONENT_NAME ObstacleAwarePlatformUnit
+ HEADERS ObstacleAwarePlatformUnit.h
+ SOURCES ObstacleAwarePlatformUnit.cpp
+ COMPONENT_LIBS ArmarXCoreComponentPlugins
+ RobotAPICore
+ RobotAPIComponentPlugins
+ RobotUnit
+)
+armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2acb4a23b423df17ca7f80ffc76251634bcb933f
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
@@ -0,0 +1,728 @@
+/*
+ * 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::ObstacleAwarePlatformUnit
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include <RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h>
+
+
+// STD/STL
+#include <algorithm>
+#include <cmath>
+#include <limits>
+#include <numeric>
+
+// Eigen
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+// Simox
+#include <SimoxUtility/math.h>
+
+// ArmarX
+#include <ArmarXCore/observers/variant/Variant.h>
+
+
+namespace
+{
+
+ void
+ invalidate(float& v)
+ {
+ v = std::numeric_limits<float>::infinity();
+ }
+
+
+ void
+ invalidate(Eigen::Vector2f& v)
+ {
+ v.x() = std::numeric_limits<float>::infinity();
+ v.y() = std::numeric_limits<float>::infinity();
+ }
+
+
+ void
+ invalidate(Eigen::Vector3f& v)
+ {
+ v.x() = std::numeric_limits<float>::infinity();
+ v.y() = std::numeric_limits<float>::infinity();
+ v.z() = std::numeric_limits<float>::infinity();
+ }
+
+ template<typename T>
+ void invalidate(std::deque<T>& d)
+ {
+ d.clear();
+ }
+
+
+ std::string
+ to_string(armarx::ObstacleAwarePlatformUnit::control_mode mode)
+ {
+ switch (mode)
+ {
+ case armarx::ObstacleAwarePlatformUnit::control_mode::position:
+ return "position";
+ case armarx::ObstacleAwarePlatformUnit::control_mode::velocity:
+ return "velocity";
+ case armarx::ObstacleAwarePlatformUnit::control_mode::none:
+ default:
+ return "unset";
+ }
+ }
+
+}
+
+
+const std::string
+armarx::ObstacleAwarePlatformUnit::default_name = "ObstacleAwarePlatformUnit";
+
+
+armarx::ObstacleAwarePlatformUnit::ObstacleAwarePlatformUnit() = default;
+
+
+armarx::ObstacleAwarePlatformUnit::~ObstacleAwarePlatformUnit() = default;
+
+
+void
+armarx::ObstacleAwarePlatformUnit::onInitPlatformUnit()
+{
+ ARMARX_DEBUG << "Initializing " << getName() << ".";
+
+ ARMARX_DEBUG << "Initialized " << getName() << ".";
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::onStartPlatformUnit()
+{
+ ARMARX_DEBUG << "Starting " << getName() << ".";
+
+ if (!hasRobot("robot"))
+ {
+ m_robot = addRobot("robot", VirtualRobot::RobotIO::eStructure);
+ }
+
+ invalidate(m_control_data.target_vel);
+ invalidate(m_control_data.target_rot_vel);
+ invalidate(m_control_data.target_pos);
+ invalidate(m_control_data.target_ori);
+ invalidate(m_viz.start);
+
+ ARMARX_DEBUG << "Started " << getName() << ".";
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::onExitPlatformUnit()
+{
+ ARMARX_DEBUG << "Exiting " << getName() << ".";
+
+ schedule_high_level_control_loop(control_mode::none);
+
+ ARMARX_DEBUG << "Exited " << getName() << ".";
+}
+
+
+std::string
+armarx::ObstacleAwarePlatformUnit::getDefaultName()
+const
+{
+ return default_name;
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::moveTo(
+ const float target_pos_x,
+ const float target_pos_y,
+ const float target_ori,
+ const float pos_reached_threshold,
+ const float ori_reached_threshold,
+ const Ice::Current&)
+{
+ using namespace simox::math;
+
+ std::scoped_lock l{m_control_data.mutex};
+
+ m_control_data.target_pos = Eigen::Vector2f{target_pos_x, target_pos_y};
+ m_control_data.target_ori = periodic_clamp<float>(target_ori, -M_PI, M_PI);
+ m_control_data.pos_reached_threshold = pos_reached_threshold;
+ m_control_data.ori_reached_threshold = ori_reached_threshold;
+
+ invalidate(m_control_data.target_vel);
+ invalidate(m_control_data.target_rot_vel);
+
+ schedule_high_level_control_loop(control_mode::position);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::move(
+ const float target_vel_x,
+ const float target_vel_y,
+ const float target_rot_vel,
+ const Ice::Current&)
+{
+ using namespace simox::math;
+
+ std::scoped_lock l{m_control_data.mutex};
+
+ m_control_data.target_vel = Eigen::Vector2f{target_vel_x, target_vel_y};
+ m_control_data.target_rot_vel = periodic_clamp<float>(target_rot_vel, -M_PI, M_PI);
+
+ invalidate(m_control_data.target_pos);
+ invalidate(m_control_data.target_ori);
+
+ ARMARX_CHECK(m_control_data.target_vel.allFinite());
+ ARMARX_CHECK(std::isfinite(m_control_data.target_rot_vel));
+
+ schedule_high_level_control_loop(control_mode::velocity);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::moveRelative(
+ const float target_pos_delta_x,
+ const float target_pos_delta_y,
+ const float target_delta_ori,
+ const float pos_reached_threshold,
+ const float ori_reached_threshold,
+ const Ice::Current& current)
+{
+ using namespace simox::math;
+
+ // Transform relative to global.
+ std::unique_lock lock{m_control_data.mutex};
+ synchronizeLocalClone(m_robot);
+ const Eigen::Vector2f agent_pos = m_robot->getGlobalPosition().head<2>();
+ const float agent_ori = mat4f_to_rpy(m_robot->getGlobalPose()).z();
+ lock.unlock();
+
+ // Use moveTo.
+ moveTo(
+ agent_pos.x() + target_pos_delta_x,
+ agent_pos.y() + target_pos_delta_y,
+ agent_ori + target_delta_ori,
+ pos_reached_threshold,
+ ori_reached_threshold,
+ current);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::setMaxVelocities(
+ const float max_pos_vel,
+ const float max_rot_vel,
+ const Ice::Current&)
+{
+ std::scoped_lock l{m_control_data.mutex};
+ m_control_data.max_vel = max_pos_vel;
+ m_control_data.max_rot_vel = max_rot_vel;
+ m_platform->setMaxVelocities(max_pos_vel, max_rot_vel);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::stopPlatform(const Ice::Current&)
+{
+ schedule_high_level_control_loop(control_mode::none);
+ m_platform->stopPlatform();
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::schedule_high_level_control_loop(control_mode mode)
+{
+ std::scoped_lock l{m_control_loop.mutex};
+
+ // If the control mode didn't change, there's nothing to do.
+ if (m_control_loop.mode == mode)
+ {
+ return;
+ }
+
+ // If a control loop is runnung, stop it and wait for termination.
+ if (m_control_loop.mode != mode and m_control_loop.task)
+ {
+ ARMARX_VERBOSE << "Stopping " << ::to_string(m_control_loop.mode) << " control.";
+ const bool join = true;
+ m_control_loop.task->stop(join);
+ }
+
+ // If the new control mode is none, no new control loop needs to be created.
+ if (mode == control_mode::none)
+ {
+ ARMARX_VERBOSE << "Going into stand-by.";
+ return;
+ }
+
+ // Start next control loop.
+ ARMARX_VERBOSE << "Starting " << ::to_string(mode) << " control.";
+ m_control_loop.mode = mode;
+ m_control_loop.task = new RunningTask<ObstacleAwarePlatformUnit>(
+ this,
+ &ObstacleAwarePlatformUnit::high_level_control_loop);
+ m_control_loop.task->start();
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::high_level_control_loop()
+{
+ const control_mode mode = m_control_loop.mode;
+ ARMARX_VERBOSE << "Started " << ::to_string(mode) << " control.";
+
+ try
+ {
+ CycleUtil cu{m_control_loop.cycle_time};
+ while (not m_control_loop.task->isStopped())
+ {
+ const velocities vels = get_velocities();
+
+ // Debug.
+ StringVariantBaseMap m;
+ m["err_dist"] = new Variant{vels.err_dist};
+ m["err_angular_dist"] = new Variant{vels.err_angular_dist};
+
+ m["target_global_x"] = new Variant{vels.target_global.x()};
+ m["target_global_y"] = new Variant{vels.target_global.y()};
+ m["target_global_abs"] = new Variant{vels.target_global.norm()};
+
+ m["target_local_x"] = new Variant{vels.target_local.x()};
+ m["target_local_y"] = new Variant{vels.target_local.y()};
+ m["target_local_abs"] = new Variant(vels.target_local.norm());
+ m["target_rot"] = new Variant{vels.target_rot};
+
+ m["modulated_global_x"] = new Variant{vels.modulated_global.x()};
+ m["modulated_global_y"] = new Variant{vels.modulated_global.y()};
+ m["modulated_global_abs"] = new Variant{vels.modulated_global.norm()};
+
+ m["modulated_local_x"] = new Variant{vels.modulated_local.x()};
+ m["modulated_local_y"] = new Variant{vels.modulated_local.y()};
+ m["modulated_local_abs"] = new Variant{vels.modulated_local.norm()};
+
+ setDebugObserverChannel("ObstacleAvoidingPlatformCtrl", m);
+
+ m_platform->move(vels.modulated_local.x(), vels.modulated_local.y(), vels.target_rot);
+ visualize(vels);
+ cu.waitForCycleDuration();
+ }
+ }
+ catch (const std::exception& e)
+ {
+ ARMARX_ERROR << "Error occured while running control loop.\n"
+ << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Unknown error occured while running control loop.";
+ }
+
+ m_platform->move(0, 0, 0);
+ m_platform->stopPlatform();
+ m_control_loop.mode = control_mode::none;
+
+ visualize();
+
+ ARMARX_VERBOSE << "Stopped " << ::to_string(mode) << " control.";
+}
+
+
+armarx::ObstacleAwarePlatformUnit::velocities
+armarx::ObstacleAwarePlatformUnit::get_velocities()
+{
+ using namespace simox::math;
+
+ // Acquire control_data mutex to ensure data remains consistent.
+ std::scoped_lock l{m_control_data.mutex};
+ // Update agent and get target velocities.
+ update_agent_dependent_values();
+ const Eigen::Vector2f target_vel = get_target_velocity();
+ const float target_rot_vel = get_target_rotational_velocity();
+
+ const Eigen::Vector2f extents = Eigen::Affine3f(m_robot->getRobotNode("PlatformCornerFrontLeft")->getPoseInRootFrame()).translation().head<2>();
+
+ // Apply obstacle avoidance and apply post processing to get the modulated velocity.
+ const Eigen::Vector2f modulated_vel = [this, &target_vel, &extents]
+ {
+ const VirtualRobot::Circle circle = VirtualRobot::projectedBoundingCircle(*m_robot);
+ ARMARX_DEBUG << "Circle approximation: " << circle.radius;
+ m_viz.boundingCircle = circle;
+ const float distance = m_obsman->distanceToObstacle(m_control_data.agent_pos /*circle.center*/, circle.radius, m_control_data.target_pos);
+
+ ARMARX_DEBUG << "Distance to obstacle: " << distance;
+
+ // https://www.wolframalpha.com/input/?i=line+through+%281000%2C+0%29+and+%282000%2C+1%29
+ float modifier = std::clamp((distance / 1000) - 1., 0.0, 1.0);
+
+ const Eigen::Vector2f vel = modifier * target_vel;
+ return vel.norm() > 20 ? vel : Eigen::Vector2f{0, 0};
+ }();
+
+ ARMARX_CHECK(modulated_vel.allFinite()) << "Velocity contains non-finite values.";
+ ARMARX_CHECK(std::isfinite(target_rot_vel)) << "Rotation velocity is non-finite.";
+
+ ARMARX_DEBUG << "Target velocity: " << target_vel.transpose()
+ << "; norm: " << target_vel.norm() << "; " << target_rot_vel;
+ ARMARX_DEBUG << "Modulated velocity: " << modulated_vel.transpose() << modulated_vel.norm();
+
+ const auto r = Eigen::Rotation2D(m_control_data.agent_ori).toRotationMatrix().inverse();
+
+ velocities vels;
+ vels.target_local = r * target_vel;
+ vels.target_global = target_vel;
+ vels.modulated_local = r * modulated_vel;
+ vels.modulated_global = modulated_vel;
+ vels.target_rot = target_rot_vel;
+ vels.err_dist = m_control_data.target_dist;
+ vels.err_angular_dist = m_control_data.target_angular_dist;
+
+ return vels;
+}
+
+
+Eigen::Vector2f
+armarx::ObstacleAwarePlatformUnit::get_target_velocity()
+const
+{
+ using namespace simox::math;
+
+ Eigen::Vector2f uncapped_target_vel = Eigen::Vector2f::Zero();
+
+ if (m_control_loop.mode == control_mode::position /*and not target_position_reached()*/)
+ {
+ uncapped_target_vel =
+ (m_control_data.target_pos - m_control_data.agent_pos) * m_control_data.kp;
+ }
+ else if (m_control_loop.mode == control_mode::velocity)
+ {
+ uncapped_target_vel = m_control_data.target_vel;
+ }
+
+ ARMARX_CHECK(uncapped_target_vel.allFinite());
+
+ return norm_max(uncapped_target_vel, m_control_data.max_vel);
+}
+
+
+float
+armarx::ObstacleAwarePlatformUnit::get_target_rotational_velocity()
+const
+{
+ using namespace simox::math;
+
+ float uncapped_target_rot_vel = 0;
+
+ if (m_control_loop.mode == control_mode::position /*and not target_orientation_reached()*/)
+ {
+ m_rot_pid_controller.update(m_control_data.target_angular_dist, 0);
+ uncapped_target_rot_vel = -m_rot_pid_controller.getControlValue();
+ }
+ else if (m_control_loop.mode == control_mode::velocity)
+ {
+ uncapped_target_rot_vel = m_control_data.target_rot_vel;
+ }
+
+ ARMARX_CHECK(std::isfinite(uncapped_target_rot_vel));
+
+ return std::copysign(std::min(std::fabs(uncapped_target_rot_vel), m_control_data.max_rot_vel),
+ uncapped_target_rot_vel);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::update_agent_dependent_values()
+{
+ using namespace simox::math;
+
+ synchronizeLocalClone(m_robot);
+ m_control_data.agent_pos = m_robot->getGlobalPosition().head<2>();
+ m_control_data.agent_ori =
+ periodic_clamp<float>(mat4f_to_rpy(m_robot->getGlobalPose()).z(), -M_PI, M_PI);
+
+ ARMARX_CHECK_GREATER(m_control_data.agent_ori, -M_PI);
+ ARMARX_CHECK_LESS(m_control_data.agent_ori, M_PI);
+
+ // Update distances in position mode.
+ if (m_control_loop.mode == control_mode::position)
+ {
+ ARMARX_CHECK_GREATER(m_control_data.target_ori, -M_PI);
+ ARMARX_CHECK_LESS(m_control_data.target_ori, M_PI);
+ ARMARX_CHECK(m_control_data.target_pos.allFinite());
+ ARMARX_CHECK(std::isfinite(m_control_data.target_ori));
+
+ m_control_data.target_dist =
+ (m_control_data.target_pos - m_control_data.agent_pos).norm();
+ m_control_data.target_angular_dist =
+ periodic_clamp<float>(m_control_data.target_ori - m_control_data.agent_ori,
+ -M_PI, M_PI);
+
+ ARMARX_CHECK_GREATER(m_control_data.target_angular_dist, -M_PI);
+ ARMARX_CHECK_LESS(m_control_data.target_angular_dist, M_PI);
+
+ ARMARX_DEBUG << "Distance to target: " << m_control_data.target_dist << " mm and "
+ << m_control_data.target_angular_dist << " rad.";
+ }
+ // Otherwise invalidate them.
+ else
+ {
+ invalidate(m_control_data.target_dist);
+ invalidate(m_control_data.target_angular_dist);
+ }
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::target_position_reached()
+const
+{
+ if (m_control_loop.mode == control_mode::position)
+ {
+ return m_control_data.target_dist < m_control_data.pos_reached_threshold;
+ }
+
+ // Cannot reach any target in non-position mode.
+ return false;
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::target_orientation_reached()
+const
+{
+ if (m_control_loop.mode == control_mode::position)
+ {
+ return std::fabs(m_control_data.target_angular_dist) < m_control_data.ori_reached_threshold;
+ }
+
+ // Cannot reach any target in non-position mode.
+ return false;
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::target_reached()
+const
+{
+ if (m_control_loop.mode == control_mode::position)
+ {
+ return target_position_reached() and target_orientation_reached();
+ }
+
+ return false;
+}
+
+
+bool
+armarx::ObstacleAwarePlatformUnit::is_near_target(const Eigen::Vector2f& control_velocity)
+const noexcept
+{
+ if (m_control_data.target_dist < m_control_data.pos_near_threshold)
+ {
+ const Eigen::Vector2f target_direction = m_control_data.target_pos - m_control_data.agent_pos;
+ const Eigen::Vector2f control_direction = control_velocity / control_velocity.norm();
+
+ const float sim = simox::math::cosine_similarity(target_direction, control_direction);
+
+ // if almost pointing into same direction
+ if (sim > cos(M_PI_4f32))
+ {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::visualize()
+{
+ const Eigen::Vector2f zero = Eigen::Vector2f::Zero();
+ velocities vels;
+ vels.target_local = zero;
+ vels.target_global = zero;
+ vels.modulated_local = zero;
+ vels.modulated_global = zero;
+ vels.target_rot = 0;
+
+ visualize(vels);
+}
+
+
+void
+armarx::ObstacleAwarePlatformUnit::visualize(const velocities& vels)
+{
+ using namespace armarx::viz;
+
+ if (not m_viz.enabled)
+ {
+ return;
+ }
+
+ Eigen::Vector3f agent_pos{m_control_data.agent_pos.x(), m_control_data.agent_pos.y(), 0};
+
+ // Progress. Only draw in position control mode.
+ Layer l_prog = arviz.layer("progress");
+ if (m_control_loop.mode == control_mode::position)
+ {
+ const float min_keypoint_dist = 50;
+
+ {
+ l_prog.add(Cylinder{"boundingCylinder"}
+ .position(agent_pos)
+ .color(Color::cyan(255, 64))
+ .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0))
+ .radius(m_viz.boundingCircle.radius)
+ .height(2000));
+ }
+
+ // If no start is set, use current agent pos.
+ if (not m_viz.start.allFinite())
+ {
+ m_viz.start = agent_pos;
+ }
+
+ const Eigen::Vector3f& last_keypoint_pos =
+ m_viz.path.size() >= 1 ? m_viz.path.back() : m_viz.start;
+
+ // Keep a certain distance between path keypoints.
+ if ((last_keypoint_pos - agent_pos).norm() > min_keypoint_dist)
+ {
+ m_viz.path.push_back(agent_pos);
+ }
+
+ // Draw path.
+ if (not target_reached())
+ {
+ // Start.
+ l_prog.add(Sphere{"start"}
+ .position(m_viz.start)
+ .color(Color::cyan(255, 64))
+ .radius(40));
+
+ // Path.
+ for (unsigned i = 0; i < m_viz.path.size(); ++i)
+ {
+ l_prog.add(Sphere{"path_" + std::to_string(i + 1)}
+ .position(m_viz.path[i])
+ .color(Color::magenta(255, 128))
+ .radius(20));
+ }
+
+ // Goal.
+ const Eigen::Vector3f target{m_control_data.target_pos.x(),
+ m_control_data.target_pos.y(),
+ 0};
+ l_prog.add(Arrow{"goal"}
+ .fromTo(target + Eigen::Vector3f{0, 0, 220},
+ target + Eigen::Vector3f{0, 0, 40})
+ .color(Color::red(255, 128))
+ .width(20));
+ }
+ else
+ {
+ m_viz.path.clear();
+ invalidate(m_viz.start);
+ }
+ }
+
+ // Velocities.
+ Layer l_vels = arviz.layer("velocities");
+ if (m_control_loop.mode != control_mode::none)
+ {
+ const float min_velocity = 15;
+ const Eigen::Vector3f from1{agent_pos + Eigen::Vector3f{0, 0, 2000}};
+ const Eigen::Vector3f from2 = from1 + Eigen::Vector3f{0, 0, 200};
+ const Eigen::Vector3f original{vels.target_global.x(), vels.target_global.y(), 0};
+ const Eigen::Vector3f modulated{vels.modulated_global.x(), vels.modulated_global.y(), 0};
+
+ if (original.norm() > min_velocity)
+ {
+ l_vels.add(Arrow{"original"}
+ .fromTo(from1, from1 + original * 5)
+ .color(Color::green(255, 128))
+ .width(25));
+ }
+
+ if (modulated.norm() > min_velocity)
+ {
+ l_vels.add(Arrow{"modulated"}
+ .fromTo(from2, from2 + modulated * 5)
+ .color(Color::cyan(255, 128))
+ .width(25));
+ }
+ }
+
+ // Agent.
+ Layer l_agnt = arviz.layer("agent");
+ if (m_control_loop.mode != control_mode::none)
+ {
+ l_agnt.add(Sphere{"agent"}
+ .position(agent_pos)
+ .color(Color::red(255, 128))
+ .radius(50));
+
+ // Agent safety margin.
+ if (m_control_data.agent_safety_margin > 0)
+ {
+ l_agnt.add(Cylinder{"agent_safety_margin"}
+ .pose(simox::math::pos_rpy_to_mat4f(agent_pos, -M_PI_2, 0, 0))
+ .color(Color::red(255, 64))
+ .radius(m_control_data.agent_safety_margin)
+ .height(2));
+ }
+ }
+
+ arviz.commit({l_prog, l_vels, l_agnt});
+}
+
+
+armarx::PropertyDefinitionsPtr
+armarx::ObstacleAwarePlatformUnit::createPropertyDefinitions()
+{
+ PropertyDefinitionsPtr def = PlatformUnit::createPropertyDefinitions();
+
+ def->component(m_platform, "Platform");
+ def->component(m_obsman, "ObstacleAvoidingPlatformUnit");
+
+ // Settings.
+ def->optional(m_control_data.min_vel_near_target, "min_vel_near_target", "Velocity in [mm/s] "
+ "the robot should at least set when near the target");
+ def->optional(m_control_data.min_vel_general, "min_vel_general", "Velocity in [mm/s] the robot "
+ "should at least set on general");
+ def->optional(m_control_data.pos_near_threshold, "pos_near_threshold", "Distance in [mm] after "
+ "which the robot is considered to be near the target for min velocity, "
+ "smoothing, etc.");
+
+ // Control parameters.
+ def->optional(m_control_data.kp, "control.pos.kp");
+ def->optional(m_rot_pid_controller.Kp, "control.rot.kp");
+ def->optional(m_rot_pid_controller.Ki, "control.rot.ki");
+ def->optional(m_rot_pid_controller.Kd, "control.rot.kd");
+ def->optional(m_control_loop.cycle_time, "control.pose.cycle_time", "Control loop cycle time.");
+
+ // Obstacle avoidance parameter.
+ def->optional(m_control_data.agent_safety_margin, "doa.agent_safety_margin");
+
+ return def;
+}
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..274b67ea153eec20473b67fa4f45b8425c7b174d
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h
@@ -0,0 +1,259 @@
+/*
+ * 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::ObstacleAwarePlatformUnit
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+// STD/STL
+#include <deque>
+#include <string>
+#include <tuple>
+#include <mutex>
+#include <vector>
+
+// Eigen
+#include <Eigen/Core>
+
+// Ice
+#include <IceUtil/Time.h>
+
+// Simox
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Safety.h>
+
+// ArmarX
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+#include <ArmarXCore/util/tasks.h>
+#include <ArmarXCore/util/time.h>
+
+// RobotAPI
+#include <RobotAPI/components/units/PlatformUnit.h>
+#include <RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+
+
+namespace armarx
+{
+
+ class ObstacleAwarePlatformUnit :
+ virtual public PlatformUnit,
+ virtual public RobotStateComponentPluginUser,
+ virtual public ArVizComponentPluginUser,
+ virtual public DebugObserverComponentPluginUser
+ {
+
+ public:
+
+ enum class control_mode
+ {
+ position,
+ velocity,
+ none
+ };
+
+ private:
+
+ struct control_loop
+ {
+ std::mutex mutex;
+ control_mode mode = control_mode::none;
+ RunningTask<ObstacleAwarePlatformUnit>::pointer_type task;
+ IceUtil::Time cycle_time = IceUtil::Time::milliSeconds(10);
+ };
+
+ struct control_data
+ {
+ std::mutex mutex;
+ Eigen::Vector2f target_pos;
+ float target_ori;
+ Eigen::Vector2f target_vel;
+ float target_rot_vel;
+ float target_dist;
+ float target_angular_dist;
+ Eigen::Vector2f agent_pos;
+ float agent_ori;
+ double agent_safety_margin = 0;
+ float min_vel_near_target = 50;
+ float min_vel_general = 100;
+ float max_vel = 200;
+ float max_rot_vel = 0.3;
+ float pos_near_threshold = 250;
+ float pos_reached_threshold = 8;
+ float ori_reached_threshold = 0.1;
+ float kp = 3.5;
+ };
+
+ struct visualization
+ {
+ Eigen::Vector3f start;
+ std::vector<Eigen::Vector3f> path;
+ bool enabled = true;
+ VirtualRobot::Circle boundingCircle;
+ };
+
+ struct velocities
+ {
+ Eigen::Vector2f target_local;
+ Eigen::Vector2f modulated_local;
+ Eigen::Vector2f target_global;
+ Eigen::Vector2f modulated_global;
+ float target_rot;
+ float err_dist;
+ float err_angular_dist;
+ };
+
+ public:
+
+ ObstacleAwarePlatformUnit();
+
+ ~ObstacleAwarePlatformUnit()
+ override;
+
+ std::string
+ getDefaultName()
+ const override;
+
+ void
+ moveTo(
+ float target_pos_x,
+ float target_pos_y,
+ float target_ori,
+ float pos_reached_threshold,
+ float ori_reached_threshold,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ void
+ move(
+ float target_vel_x,
+ float target_vel_y,
+ float target_rot_vel,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ void
+ moveRelative(
+ float target_pos_delta_x,
+ float target_pos_delta_y,
+ float target_delta_ori,
+ float pos_reached_threshold,
+ float ori_reached_threshold,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ void
+ setMaxVelocities(
+ float max_pos_vel,
+ float max_rot_vel,
+ const Ice::Current& = Ice::Current{})
+ override;
+
+ void
+ stopPlatform(const Ice::Current& = Ice::Current{})
+ override;
+
+ protected:
+
+ void
+ onInitPlatformUnit()
+ override;
+
+ void
+ onStartPlatformUnit()
+ override;
+
+ void
+ onExitPlatformUnit()
+ override;
+
+ PropertyDefinitionsPtr
+ createPropertyDefinitions()
+ override;
+
+ private:
+
+ void
+ schedule_high_level_control_loop(control_mode mode);
+
+ void
+ high_level_control_loop();
+
+ velocities
+ get_velocities();
+
+ void
+ update_agent_dependent_values();
+
+ Eigen::Vector2f
+ get_target_velocity()
+ const;
+
+ float
+ get_target_rotational_velocity()
+ const;
+
+ bool
+ target_position_reached()
+ const;
+
+ bool
+ target_orientation_reached()
+ const;
+
+ bool
+ target_reached()
+ const;
+
+ void
+ visualize();
+
+ void
+ visualize(const velocities& vels);
+
+ bool
+ is_near_target(const Eigen::Vector2f& control_velocity)
+ const
+ noexcept;
+
+ public:
+
+ static const std::string default_name;
+
+ private:
+
+ PlatformUnitInterfacePrx m_platform;
+ VirtualRobot::RobotPtr m_robot;
+ DynamicObstacleManagerInterfacePrx m_obsman;
+
+ ObstacleAwarePlatformUnit::control_loop m_control_loop;
+ ObstacleAwarePlatformUnit::control_data m_control_data;
+
+ mutable PIDController m_rot_pid_controller{1.0, 0.00009, 0.0};
+
+ visualization m_viz;
+
+ };
+
+}
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f168208f6734ea228aa848dcf0c5d0976370bae7
--- /dev/null
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/main.cpp
@@ -0,0 +1,40 @@
+/*
+ * 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::ObstacleAwarePlatformUnit
+ * @author Christian R. G. Dreher <c.dreher@kit.edu>
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+// STD/STL
+#include <string>
+
+// ArmarX
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/Component.h>
+
+// RobotAPI
+#include <RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.h>
+
+
+int main(int argc, char* argv[])
+{
+ using namespace armarx;
+ const std::string name = ObstacleAwarePlatformUnit::default_name;
+ return runSimpleComponentApp<ObstacleAwarePlatformUnit>(argc, argv, name);
+}
diff --git a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
index d21a3c55b7574a459f0be3e2577870cf7640f5e3..5ff2649fae12cb906f5821f73ba1fb1fd21647fe 100644
--- a/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
+++ b/source/RobotAPI/interface/components/ObstacleAvoidance/DynamicObstacleManagerInterface.ice
@@ -30,6 +30,18 @@
module armarx
{
+ module dynamicobstaclemanager
+ {
+ struct LineSegment
+ {
+ Eigen::Vector2f lineStart;
+ Eigen::Vector2f lineEnd;
+ };
+
+ sequence<LineSegment> LineSegments;
+ }
+
+
interface DynamicObstacleManagerInterface
{
void
@@ -38,6 +50,9 @@ module armarx
void
add_decayable_line_segment(Eigen::Vector2f line_start, Eigen::Vector2f line_end);
+ void
+ add_decayable_line_segments(dynamicobstaclemanager::LineSegments lines);
+
void
remove_all_decayable_obstacles();
@@ -48,6 +63,8 @@ module armarx
remove_obstacle(string name);
void wait_unitl_obstacles_are_ready();
+
+ float distanceToObstacle(Eigen::Vector2f agentPosition, float safetyRadius, Eigen::Vector2f goal);
};
};