test on ARMAR 6

scenarios/PlatformNavigation/PlatformNavigation.scx

@@ -8,3 +8,4 @@
 	<application name="ExampleClient" instance="" package="Navigation" nodeName="" enabled="false" iceAutoRestart="false"/>
 	<application name="NavigationMemory" instance="" package="Navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
 </scenario>
+

scenarios/PlatformNavigation/config/ExampleClient.cfg

@@ -26,7 +26,7 @@
 # ArmarX.CachePath = mongo/.cache
 
 
-# ArmarX.Config:  Comma-separated list of configuration files
+# ArmarX.Config:  Comma-separated list of configuration files 
 #  Attributes:
 #  - Default:            ""
 #  - Case sensitivity:   yes
@@ -192,3 +192,5 @@
 #  - Required:           no
 #  - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
 # ArmarX.Verbosity = Info
+
+

scenarios/PlatformNavigation/config/MemoryNameSystem.cfg

@@ -26,7 +26,7 @@
 # ArmarX.CachePath = mongo/.cache
 
 
-# ArmarX.Config:  Comma-separated list of configuration files
+# ArmarX.Config:  Comma-separated list of configuration files 
 #  Attributes:
 #  - Default:            ""
 #  - Case sensitivity:   yes
@@ -192,3 +192,5 @@
 #  - Required:           no
 #  - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
 # ArmarX.Verbosity = Info
+
+

scenarios/PlatformNavigation/config/NavigationMemory.cfg

@@ -26,7 +26,7 @@
 # ArmarX.CachePath = mongo/.cache
 
 
-# ArmarX.Config:  Comma-separated list of configuration files
+# ArmarX.Config:  Comma-separated list of configuration files 
 #  Attributes:
 #  - Default:            ""
 #  - Case sensitivity:   yes
@@ -118,7 +118,7 @@
 # ArmarX.NavigationMemory.ObjectName = ""
 
 
-# ArmarX.NavigationMemory.ltm.00_enabled:
+# ArmarX.NavigationMemory.ltm.00_enabled:  
 #  Attributes:
 #  - Default:            true
 #  - Case sensitivity:   yes
@@ -127,7 +127,7 @@
 # ArmarX.NavigationMemory.ltm.00_enabled = true
 
 
-# ArmarX.NavigationMemory.ltm.10_host:
+# ArmarX.NavigationMemory.ltm.10_host:  
 #  Attributes:
 #  - Default:            localhost
 #  - Case sensitivity:   yes
@@ -135,7 +135,7 @@
 # ArmarX.NavigationMemory.ltm.10_host = localhost
 
 
-# ArmarX.NavigationMemory.ltm.11_port:
+# ArmarX.NavigationMemory.ltm.11_port:  
 #  Attributes:
 #  - Default:            25270
 #  - Case sensitivity:   yes
@@ -143,7 +143,7 @@
 # ArmarX.NavigationMemory.ltm.11_port = 25270
 
 
-# ArmarX.NavigationMemory.ltm.20_user:
+# ArmarX.NavigationMemory.ltm.20_user:  
 #  Attributes:
 #  - Default:            ""
 #  - Case sensitivity:   yes
@@ -151,7 +151,7 @@
 # ArmarX.NavigationMemory.ltm.20_user = ""
 
 
-# ArmarX.NavigationMemory.ltm.21_password:
+# ArmarX.NavigationMemory.ltm.21_password:  
 #  Attributes:
 #  - Default:            ""
 #  - Case sensitivity:   yes
@@ -159,7 +159,7 @@
 # ArmarX.NavigationMemory.ltm.21_password = ""
 
 
-# ArmarX.NavigationMemory.ltm.22_database:
+# ArmarX.NavigationMemory.ltm.22_database:  
 #  Attributes:
 #  - Default:            Test
 #  - Case sensitivity:   yes
@@ -267,3 +267,5 @@
 #  - Required:           no
 #  - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
 # ArmarX.Verbosity = Info
+
+

scenarios/PlatformNavigation/config/Navigator.cfg

@@ -26,7 +26,7 @@
 # ArmarX.CachePath = mongo/.cache
 
 
-# ArmarX.Config:  Comma-separated list of configuration files
+# ArmarX.Config:  Comma-separated list of configuration files 
 #  Attributes:
 #  - Default:            ""
 #  - Case sensitivity:   yes
@@ -158,7 +158,7 @@ ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
 # ArmarX.Navigator.cmp.RemoteGui = RemoteGuiProvider
 
 
-# ArmarX.Navigator.mem.nav.stack_result.CoreSegment:
+# ArmarX.Navigator.mem.nav.stack_result.CoreSegment:  
 #  Attributes:
 #  - Default:            ""
 #  - Case sensitivity:   yes
@@ -166,7 +166,7 @@ ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
 # ArmarX.Navigator.mem.nav.stack_result.CoreSegment = ""
 
 
-# ArmarX.Navigator.mem.nav.stack_result.Memory:
+# ArmarX.Navigator.mem.nav.stack_result.Memory:  
 #  Attributes:
 #  - Default:            Navigation
 #  - Case sensitivity:   yes
@@ -176,9 +176,8 @@ ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
 
 # ArmarX.Navigator.mem.nav.stack_result.Provider:  Name of this provider
 #  Attributes:
-#  - Default:            ""
 #  - Case sensitivity:   yes
-#  - Required:           no
+#  - Required:           yes
 ArmarX.Navigator.mem.nav.stack_result.Provider = unused
 
 
@@ -274,3 +273,5 @@ ArmarX.Navigator.mem.nav.stack_result.Provider = unused
 #  - Required:           no
 #  - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
 # ArmarX.Verbosity = Info
+
+

scenarios/PlatformNavigation/config/ObjectMemory.cfg

@@ -26,7 +26,7 @@
 # ArmarX.CachePath = mongo/.cache
 
 
-# ArmarX.Config:  Comma-separated list of configuration files
+# ArmarX.Config:  Comma-separated list of configuration files 
 #  Attributes:
 #  - Default:            ""
 #  - Case sensitivity:   yes
@@ -150,7 +150,7 @@
 # ArmarX.ObjectMemory.cmp.KinematicUnitObserverName = KinematicUnitObserver
 
 
-# ArmarX.ObjectMemory.ltm.00_enabled:
+# ArmarX.ObjectMemory.ltm.00_enabled:  
 #  Attributes:
 #  - Default:            true
 #  - Case sensitivity:   yes
@@ -159,7 +159,7 @@
 # ArmarX.ObjectMemory.ltm.00_enabled = true
 
 
-# ArmarX.ObjectMemory.ltm.10_host:
+# ArmarX.ObjectMemory.ltm.10_host:  
 #  Attributes:
 #  - Default:            localhost
 #  - Case sensitivity:   yes
@@ -167,7 +167,7 @@
 # ArmarX.ObjectMemory.ltm.10_host = localhost
 
 
-# ArmarX.ObjectMemory.ltm.11_port:
+# ArmarX.ObjectMemory.ltm.11_port:  
 #  Attributes:
 #  - Default:            25270
 #  - Case sensitivity:   yes
@@ -175,7 +175,7 @@
 # ArmarX.ObjectMemory.ltm.11_port = 25270
 
 
-# ArmarX.ObjectMemory.ltm.20_user:
+# ArmarX.ObjectMemory.ltm.20_user:  
 #  Attributes:
 #  - Default:            ""
 #  - Case sensitivity:   yes
@@ -183,7 +183,7 @@
 # ArmarX.ObjectMemory.ltm.20_user = ""
 
 
-# ArmarX.ObjectMemory.ltm.21_password:
+# ArmarX.ObjectMemory.ltm.21_password:  
 #  Attributes:
 #  - Default:            ""
 #  - Case sensitivity:   yes
@@ -191,7 +191,7 @@
 # ArmarX.ObjectMemory.ltm.21_password = ""
 
 
-# ArmarX.ObjectMemory.ltm.22_database:
+# ArmarX.ObjectMemory.ltm.22_database:  
 #  Attributes:
 #  - Default:            Test
 #  - Case sensitivity:   yes
@@ -207,55 +207,6 @@
 # ArmarX.ObjectMemory.mem.MemoryName = Object
 
 
-# ArmarX.ObjectMemory.mem.articulated.cls.CoreSegmentName:  Name of the object instance core segment.
-#  Attributes:
-#  - Default:            ArticulatedObjectClass
-#  - Case sensitivity:   yes
-#  - Required:           no
-# ArmarX.ObjectMemory.mem.articulated.cls.CoreSegmentName = ArticulatedObjectClass
-
-
-# ArmarX.ObjectMemory.mem.articulated.cls.LoadFromObjectsPackage:  If true, load the objects from the objects package on startup.
-#  Attributes:
-#  - Default:            true
-#  - Case sensitivity:   yes
-#  - Required:           no
-#  - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.articulated.cls.LoadFromObjectsPackage = true
-
-
-# ArmarX.ObjectMemory.mem.articulated.cls.MaxHistorySize:  Maximal size of object poses history (-1 for infinite).
-#  Attributes:
-#  - Default:            -1
-#  - Case sensitivity:   yes
-#  - Required:           no
-# ArmarX.ObjectMemory.mem.articulated.cls.MaxHistorySize = -1
-
-
-# ArmarX.ObjectMemory.mem.articulated.cls.ObjectsPackage:  Name of the objects package to load from.
-#  Attributes:
-#  - Default:            PriorKnowledgeData
-#  - Case sensitivity:   yes
-#  - Required:           no
-# ArmarX.ObjectMemory.mem.articulated.cls.ObjectsPackage = PriorKnowledgeData
-
-
-# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName:  Name of the object instance core segment.
-#  Attributes:
-#  - Default:            ArticulatedObjectInstance
-#  - Case sensitivity:   yes
-#  - Required:           no
-# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName = ArticulatedObjectInstance
-
-
-# ArmarX.ObjectMemory.mem.articulated.inst.MaxHistorySize:  Maximal size of object poses history (-1 for infinite).
-#  Attributes:
-#  - Default:            -1
-#  - Case sensitivity:   yes
-#  - Required:           no
-# ArmarX.ObjectMemory.mem.articulated.inst.MaxHistorySize = -1
-
-
 # ArmarX.ObjectMemory.mem.attachments.CoreSegmentName:  Name of the object instance core segment.
 #  Attributes:
 #  - Default:            Attachments
@@ -288,7 +239,7 @@
 # ArmarX.ObjectMemory.mem.cls.Floor.EntityName = Building/floor-20x20
 
 
-# ArmarX.ObjectMemory.mem.cls.Floor.Height:  Height (z) of the floor plane.
+# ArmarX.ObjectMemory.mem.cls.Floor.Height:  Height (z) of the floor plane. 
 # Set slightly below 0 to avoid z-fighting when drawing planes on the ground.
 #  Attributes:
 #  - Default:            -1
@@ -675,3 +626,5 @@
 #  - Required:           no
 #  - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
 # ArmarX.Verbosity = Info
+
+

scenarios/PlatformNavigation/config/RemoteGuiProviderApp.cfg

@@ -26,7 +26,7 @@
 # ArmarX.CachePath = mongo/.cache
 
 
-# ArmarX.Config:  Comma-separated list of configuration files
+# ArmarX.Config:  Comma-separated list of configuration files 
 #  Attributes:
 #  - Default:            ""
 #  - Case sensitivity:   yes
@@ -192,3 +192,5 @@
 #  - Required:           no
 #  - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
 # ArmarX.Verbosity = Info
+
+

scenarios/PlatformNavigation/config/RobotStateComponent.cfg

@@ -26,7 +26,7 @@
 # ArmarX.CachePath = mongo/.cache
 
 
-# ArmarX.Config:  Comma-separated list of configuration files
+# ArmarX.Config:  Comma-separated list of configuration files 
 #  Attributes:
 #  - Default:            ""
 #  - Case sensitivity:   yes
@@ -312,3 +312,5 @@ ArmarX.RobotStateComponent.RobotNodeSetName = Robot
 #  - Required:           no
 #  - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
 # ArmarX.Verbosity = Info
+
+

scenarios/PlatformNavigation/config/global.cfg

 # ==================================================================
 # Global Config from Scenario PlatformNavigation
 # ==================================================================
+

source/Navigation/libraries/core/Trajectory.cpp

@@ -445,7 +445,7 @@ namespace armarx::nav::core
             pts.front().waypoint.pose,
             resampledPath,
             pts.back().waypoint.pose,
-            300.F); // TODO remove 0.F!, use proper interpolation! (via projection!)
+            500.F); // TODO remove 0.F!, use proper interpolation! (via projection!)
     }
 
     float

source/Navigation/libraries/global_planning/AStar.h

@@ -34,7 +34,7 @@ namespace armarx::nav::glob_plan
      */
     struct AStarParams : public GlobalPlannerParams
     {
-        float linearVelocity{300};
+        float linearVelocity{500};
 
         float resampleDistance{500};
 

source/Navigation/libraries/server/Navigator.cpp

@@ -214,7 +214,7 @@ namespace armarx::nav::server
         // FIXME fix angular velocity
         robotFrameVelocity.angular = res.velocity().angular;
 
-        ARMARX_INFO << deactivateSpam(100) << "velocity in robot frame "
+        ARMARX_INFO << deactivateSpam(1) << "velocity in robot frame "
                     << robotFrameVelocity.linear;
 
         config.executor->move(robotFrameVelocity);

source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp

@@ -63,7 +63,7 @@ namespace armarx::nav::traj_ctrl
                std::numeric_limits<double>::max(),
                false,
                std::vector<bool>{false, false, false}),
-        pidPosTarget(params.pidPos.Kp,
+        pidPosTarget(params.pidPos.Kp / 2,
                      params.pidPos.Ki,
                      params.pidPos.Kd,
                      std::numeric_limits<double>::max(),
@@ -170,7 +170,7 @@ namespace armarx::nav::traj_ctrl
                  projectedPose.wayPointBefore.waypoint.pose.translation())
                     .normalized();
 
-            const float ffVel = projectedPose.projection.velocity;
+            const float ffVel = 700; //projectedPose.projection.velocity;
 
             const auto feedforwardVelocity = desiredMovementDirection * ffVel;