data/armarx_navigation/controller_config/PlatformTrajectory/default.json

 {
   "params": {
     "pidPos": {
-      "Kp": 0.1,
+      "Kp": 1,
       "Ki": 0,
       "Kd": 0
     },
     "pidOri": {
-      "Kp": 0.1,
+      "Kp": 1,
       "Ki": 0,
       "Kd": 0
     },

scenarios/NavigationSimulation/NavigationSimulation.scx

@@ -39,7 +39,7 @@
 	<application name="VisionMemory" instance="" package="VisionX" nodeName="" enabled="true" iceAutoRestart="false"/>
 	<application name="HumanMemoryApp" instance="" package="VisionX" nodeName="" enabled="true" iceAutoRestart="false"/>
 	<application name="object_memory_to_simulation" instance="" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
-	<application name="SimulationObjectPoseProviderApp" instance="" package="ArmarXSimulation" nodeName="" enabled="true" iceAutoRestart="false"/>
+	<application name="SimulationObjectPoseProviderApp" instance="" package="ArmarXSimulation" nodeName="" enabled="false" iceAutoRestart="false"/>
 	<application name="control_memory" instance="" package="armarx_control" nodeName="" enabled="true" iceAutoRestart="false"/>
 </scenario>
 

scenarios/NavigationSimulation/config/RobotStateMemory.cfg

@@ -224,6 +224,22 @@ ArmarX.RobotStateMemory.RobotUnitName = Armar6Unit
 # ArmarX.RobotStateMemory.mem.desc.seg.CoreSegmentName = Description
 
 
+# ArmarX.RobotStateMemory.mem.ext.seg.CoreMaxHistorySize:  Maximal size of the Exteroception entity histories (-1 for infinite).
+#  Attributes:
+#  - Default:            -1
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.RobotStateMemory.mem.ext.seg.CoreMaxHistorySize = -1
+
+
+# ArmarX.RobotStateMemory.mem.ext.seg.CoreSegmentName:  Name of the Exteroception core segment.
+#  Attributes:
+#  - Default:            Exteroception
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.RobotStateMemory.mem.ext.seg.CoreSegmentName = Exteroception
+
+
 # ArmarX.RobotStateMemory.mem.loc.seg.CoreMaxHistorySize:  Maximal size of the Localization entity histories (-1 for infinite).
 #  Attributes:
 #  - Default:            1024

scenarios/NavigationSimulation/config/SimulatorApp.cfg

@@ -289,7 +289,7 @@ ArmarX.Simulator.InitialRobotConfig = ArmL1_Cla1:0.036781001836061478,ArmL2_Sho1
 #  - Default:            0
 #  - Case sensitivity:   yes
 #  - Required:           no
-ArmarX.Simulator.InitialRobotPose.x = 2000
+ArmarX.Simulator.InitialRobotPose.x = -1000
 
 
 # ArmarX.Simulator.InitialRobotPose.x_0:  x component of initial robot position (mm)
@@ -321,7 +321,7 @@ ArmarX.Simulator.InitialRobotPose.x = 2000
 #  - Default:            0
 #  - Case sensitivity:   yes
 #  - Required:           no
-ArmarX.Simulator.InitialRobotPose.y = 815
+ArmarX.Simulator.InitialRobotPose.y = 4500
 
 
 # ArmarX.Simulator.InitialRobotPose.y_0:  y component of initial robot position (mm)
@@ -353,7 +353,7 @@ ArmarX.Simulator.InitialRobotPose.y = 815
 #  - Default:            0
 #  - Case sensitivity:   yes
 #  - Required:           no
-# ArmarX.Simulator.InitialRobotPose.yaw = 0
+ArmarX.Simulator.InitialRobotPose.yaw = 1.57
 
 
 # ArmarX.Simulator.InitialRobotPose.yaw_0:  Initial robot pose: yaw component of RPY angles (radian)

scenarios/PlatformNavigation/config/ObjectMemory.cfg

@@ -391,9 +391,12 @@
 # ArmarX.ObjectMemory.mem.inst.scene.11_Directory = scenes
 
 
-# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad:  Scene to load on startup (e.g. 'Scene_2021-06-24_20-20-03').
-# You can also specify paths relative to 'Package/scenes/'. 
-# You can also specify a ; separated list of scenes.
+# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad:  Scene(s) to load on startup. 
+# Specify multiple scenes in a ; separated list. 
+# Each entry must be one of the following: 
+# (1) A scene file in 'Package/scenes/' (with or without '.json' extension), e.g. 'MyScene', 'MyScene.json' 
+# (2) A path to a scene file relative to 'Package/scenes/' (with or without '.json' extension), e.g. 'path/to/MyScene', 'path/to/MyScene.json' 
+# (3) An ArmarX data path to a scene file, e.g. 'Package/scenes/path/to/MyScene.json'
 #  Attributes:
 #  - Default:            ""
 #  - Case sensitivity:   yes

source/armarx/navigation/core/Trajectory.cpp

@@ -18,10 +18,11 @@
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/core/logging/Logging.h>
 
-#include "range/v3/algorithm/none_of.hpp"
 #include <armarx/navigation/conversions/eigen.h>
 #include <armarx/navigation/core/basic_types.h>
 #include <armarx/navigation/core/types.h>
+
+#include <range/v3/algorithm/none_of.hpp>
 #include <range/v3/action/insert.hpp>
 #include <range/v3/numeric/accumulate.hpp>
 #include <range/v3/range/conversion.hpp>
@@ -157,7 +158,6 @@ namespace armarx::navigation::core
 
     } // namespace conv
 
-
     GlobalTrajectory::InternalProjection
     GlobalTrajectory::projectInternal(const Position& point,
                                       const VelocityInterpolation& velocityInterpolation) const
@@ -641,7 +641,6 @@ namespace armarx::navigation::core
         return pts;
     }
 
-
     float
     GlobalTrajectory::duration(const core::VelocityInterpolation interpolation) const
     {
@@ -748,8 +747,11 @@ namespace armarx::navigation::core
     }
 
     Evaluation
-    LocalTrajectory::evaluate(const armarx::core::time::DateTime& timestamp) const
+    LocalTrajectory::evaluate(const armarx::core::time::DateTime& ts) const
     {
+        const auto& finalTimestamp = points().back().timestamp;
+        const auto timestamp = ts > finalTimestamp ? finalTimestamp - Duration::MilliSeconds(1) : ts;
+
         const auto cmp = [](const core::LocalTrajectoryPoint& lhs,
                             const DateTime& timestamp) -> bool
         { return lhs.timestamp < timestamp; };

source/armarx/navigation/global_planning/CMakeLists.txt

@@ -8,8 +8,7 @@ armarx_add_aron_library(global_planning_aron
 
 armarx_add_library(global_planning
     DEPENDENCIES_PUBLIC
-        # Ceres::ceres
-        ceres
+        Ceres::ceres
         ArmarXCoreInterfaces
         ArmarXCore
         aroncommon

source/armarx/navigation/human/test/human_tracker_test.cpp

@@ -239,7 +239,7 @@ namespace armarx::navigation::human
 
         BOOST_CHECK_EQUAL(tracker.getTrackedHumans().size(), 1);
 
-        CamMm late = {.detectionTime = DateTime(Duration::MilliSeconds(1000))};
+        CamMm late = {.detectionTime = DateTime(Duration::MilliSeconds(1000)), .humanPoses = {}};
         tracker.update(late);
 
         BOOST_CHECK_EQUAL(tracker.getTrackedHumans().size(), 0);

source/armarx/navigation/server/execution/DummyExecutor.h

@@ -20,12 +20,14 @@ namespace armarx::navigation::server
 
         ~DummyExecutor() override = default;
 
-        void
-        execute(const core::LocalTrajectory& trajectory) override
-        {
-        }
 
-        void start() override{};
+        void execute(const core::LocalTrajectory& trajectory,
+                     bool activateController = false) override{};
+
+        void execute(const core::GlobalTrajectory& trajectory,
+                     bool activateController = false) override{};
+
+        void start(ControllerType controllerType) override{};
         void stop() override{};
 
 

source/armarx/navigation/server/scene_provider/SceneProvider.cpp

@@ -55,7 +55,7 @@ namespace armarx::navigation::server::scene_provider
         scn.timestamp = timestamp;
 
         ARMARX_CHECK_NOT_NULL(srv.virtualRobotReader);
-        srv.virtualRobotReader->synchronizeRobot(*scn.robot, timestamp);
+        ARMARX_CHECK(srv.virtualRobotReader->synchronizeRobot(*scn.robot, timestamp));
 
         scn.dynamicScene = getDynamicScene(timestamp);
         // scn.graph = getSceneGraph(timestamp);

source/armarx/navigation/trajectory_control/global/TrajectoryFollowingController.cpp

 #include "TrajectoryFollowingController.h"
+#include <cmath>
 
 #include <SimoxUtility/math/convert/mat4f_to_pos.h>
 #include <SimoxUtility/math/convert/mat4f_to_rpy.h>
@@ -98,10 +99,19 @@ namespace armarx::navigation::traj_ctrl::global
         const core::Twist limits{.linear = Eigen::Vector3f::Ones() * params.limits.linear,
                                  .angular = Eigen::Vector3f::Ones() * params.limits.angular};
 
+        ARMARX_CHECK((limits.linear.array() > 0).all());
+        ARMARX_CHECK((limits.angular.array() > 0).all());
+
+        ARMARX_CHECK(limits.linear.allFinite());
+        ARMARX_CHECK(limits.angular.allFinite());
+
         // for all entries, scale should be less than 1
         const auto scalePos = twist.linear.cwiseAbs().cwiseQuotient(limits.linear.cwiseAbs());
         const auto scaleOri = twist.angular.cwiseAbs().cwiseQuotient(limits.angular.cwiseAbs());
 
+        ARMARX_CHECK(scalePos.allFinite());
+        ARMARX_CHECK(scaleOri.allFinite());
+
         const float scaleMax = std::max(scalePos.maxCoeff(), scaleOri.maxCoeff());
 
         if (scaleMax < 1.0F) // both linear and angular velocity in bounds?
@@ -109,6 +119,8 @@ namespace armarx::navigation::traj_ctrl::global
             return twist;
         }
 
+        ARMARX_CHECK(not std::isnan(scaleMax));
+
         // scale such that no limit is violated
         twist.linear /= scaleMax;
         twist.angular /= scaleMax;

source/armarx/navigation/trajectory_control/local/TrajectoryFollowingController.cpp

@@ -15,8 +15,8 @@
 
 #include <RobotAPI/libraries/core/MultiDimPIDController.h>
 
-#include <armarx/navigation/core/basic_types.h>
 #include <armarx/navigation/core/Trajectory.h>
+#include <armarx/navigation/core/basic_types.h>
 #include <armarx/navigation/core/types.h>
 #include <armarx/navigation/trajectory_control/local/TrajectoryController.h>
 #include <armarx/navigation/trajectory_control/local/aron/TrajectoryFollowingControllerParams.aron.generated.h>
@@ -102,12 +102,19 @@ namespace armarx::navigation::traj_ctrl::local
         const core::Twist limits{.linear = Eigen::Vector3f::Ones() * params.limits.linear,
                                  .angular = Eigen::Vector3f::Ones() * params.limits.angular};
 
+        ARMARX_CHECK((limits.linear.array() > 0).all());
+        ARMARX_CHECK((limits.angular.array() > 0).all());
+        ARMARX_CHECK(limits.linear.allFinite());
+        ARMARX_CHECK(limits.angular.allFinite());
+
         // for all entries, scale should be less than 1
         const auto scalePos = twist.linear.cwiseAbs().cwiseQuotient(limits.linear.cwiseAbs());
         const auto scaleOri = twist.angular.cwiseAbs().cwiseQuotient(limits.angular.cwiseAbs());
 
-        const float scaleMax = std::max(scalePos.maxCoeff(), scaleOri.maxCoeff());
+        ARMARX_CHECK(scalePos.allFinite());
+        ARMARX_CHECK(scaleOri.allFinite());
 
+        const float scaleMax = std::max(scalePos.maxCoeff(), scaleOri.maxCoeff());
         if (scaleMax < 1.0F) // both linear and angular velocity in bounds?
         {
             return twist;
@@ -158,13 +165,10 @@ namespace armarx::navigation::traj_ctrl::local
         const core::Twist& twistFeedForward = target.feedforwardTwist;
 
         const core::Twist twistError{.linear = pidPos.getControlValue(),
-                                .angular = pidOri.getControlValue()};
+                                     .angular = pidOri.getControlValue()};
 
-        const core::Twist twist
-        {
-            .linear = twistFeedForward.linear + twistError.linear,
-            .angular = twistFeedForward.angular + twistError.angular
-        };
+        const core::Twist twist{.linear = twistFeedForward.linear + twistError.linear,
+                                .angular = twistFeedForward.angular + twistError.angular};
 
         const auto twistLimited = applyTwistLimits(twist);
         ARMARX_VERBOSE << deactivateSpam(1) << "Twist limited " << twistLimited.linear.transpose();