update

source/Navigation/libraries/core/CMakeLists.txt

@@ -17,6 +17,7 @@ armarx_add_library(
     SOURCES
         StaticScene.cpp
         Trajectory.cpp
+        types.cpp
     HEADERS
         types.h
         MemoryReferencedElement.h

source/Navigation/libraries/core/types.cpp

+#include "types.h"
+
+namespace armarx::nav::core
+{
+    core::Pose Twist::poseDiff(const float dt) const
+    {
+        return core::Pose(Eigen::Translation3f(linear * dt)) *
+               core::Pose(Eigen::AngleAxisf(angular.norm() * dt, angular.normalized()));
+    }
+} // namespace armarx::nav::core

source/Navigation/libraries/core/types.h

@@ -53,6 +53,8 @@ namespace armarx::nav::core
     {
         Eigen::Vector3f linear;
         Eigen::Vector3f angular;
+
+        core::Pose poseDiff(float dt) const;
     };
 
     struct Waypoint
@@ -60,8 +62,6 @@ namespace armarx::nav::core
         Pose pose;
     };
 
-
-
     struct Scene
     {
         std::optional<core::StaticScene> staticScene = std::nullopt;

source/Navigation/libraries/global_planning/Point2Point.cpp

@@ -48,7 +48,7 @@ namespace armarx::nav::glob_plan
         {
             trajectory.push_back(core::TrajectoryPoint
             {
-                .waypoint = core::Waypoint{.pose = Eigen::Affine3f(context.robot->getGlobalPose())},
+                .waypoint = core::Waypoint{.pose = core::Pose(context.robot->getGlobalPose())},
                 .twist = {.linear = Eigen::Vector3f::Zero(), .angular = Eigen::Vector3f::Zero()}});
         }
 

source/Navigation/libraries/server/Navigator.cpp

@@ -71,7 +71,7 @@ namespace armarx::nav::server
         ARMARX_CHECK_NOT_NULL(stack.globalPlanner);
 
         StackResult res;
-        res.globalTrajectory = stack.globalPlanner->plan(Eigen::Affine3f(waypoints.at(0)));
+        res.globalTrajectory = stack.globalPlanner->plan(core::Pose(waypoints.at(0)));
 
         if (stack.localPlanner)
         {

source/Navigation/libraries/server/execution/DummyExecutor.h

@@ -42,7 +42,7 @@ namespace armarx::nav::server
 
             //lastUpdated = now;
 
-            const Eigen::Affine3f diff; // = Eigen::Affine3f(Eigen::Translation3f(twist.linear * dt)) * Eigen::Affine3f(Eigen::AngleAxisf(dt, twist.angular)));
+            const core::Pose diff; // TODO
 
             robot->setGlobalPose(robot->getGlobalPose() * diff.matrix());
         }

source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp

@@ -23,9 +23,11 @@
 #include <limits>
 
 #include <Eigen/Geometry>
+#include <Eigen/src/Geometry/AngleAxis.h>
 #include <Eigen/src/Geometry/Transform.h>
 #include <Eigen/src/Geometry/Translation.h>
 
+#include <SimoxUtility/math/convert/deg_to_rad.h>
 #include <VirtualRobot/Robot.h>
 
 #include "Navigation/libraries/core/Trajectory.h"
@@ -59,7 +61,7 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerOnTrajectory)
     traj_ctrl::TrajectoryFollowingController controller(params, scene);
 
     core::TrajectoryPoint wpBefore;
-    wpBefore.waypoint.pose = Eigen::Affine3f::Identity();
+    wpBefore.waypoint.pose = core::Pose::Identity();
     wpBefore.waypoint.pose.translation().x() -= 100.F;
     wpBefore.twist.linear = Eigen::Vector3f::UnitX();
     wpBefore.twist.angular = Eigen::Vector3f::Zero();
@@ -79,16 +81,17 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerOnTrajectory)
     BOOST_CHECK_EQUAL(true, true);
 }
 
-
 /**
- * @brief Tests the control 
- * 
+ * @brief Tests the control
+ *
  */
 BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerNextToTrajectory)
 {
     core::Scene scene;
     scene.robot = std::make_shared<VirtualRobot::LocalRobot>("foo", "bar");
-    scene.robot->setGlobalPose(Eigen::Matrix4f::Identity() * Eigen::Affine3f(Eigen::Translation3f(0, 100, 0)).matrix(), false);
+    scene.robot->setGlobalPose(Eigen::Matrix4f::Identity() *
+                               core::Pose(Eigen::Translation3f(0, 100, 0)).matrix(),
+                               false);
 
     traj_ctrl::TrajectoryFollowingControllerParams params;
     params.pidPos = {1, 0, 0};
@@ -99,7 +102,7 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerNextToTrajectory)
     traj_ctrl::TrajectoryFollowingController controller(params, scene);
 
     core::TrajectoryPoint wpBefore;
-    wpBefore.waypoint.pose = Eigen::Affine3f::Identity();
+    wpBefore.waypoint.pose = core::Pose::Identity();
     wpBefore.waypoint.pose.translation().x() -= 100.F;
     wpBefore.twist.linear = Eigen::Vector3f::UnitX();
     wpBefore.twist.angular = Eigen::Vector3f::Zero();
@@ -118,3 +121,96 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerNextToTrajectory)
 
     BOOST_CHECK_EQUAL(true, true);
 }
+
+class TargetReachedCondition
+{
+public:
+    struct Params
+    {
+        struct
+        {
+            float linear;
+            float angular;
+        } distance;
+    };
+
+    TargetReachedCondition(const core::TrajectoryPoint& point, const Params& params) :
+        point(point), params(params)
+    {
+    }
+
+    bool check(const core::Pose& pose) const
+    {
+        const float dp = (pose.translation() - point.waypoint.pose.translation()).norm();
+        const float dr =
+            Eigen::AngleAxisf(pose.linear().inverse() * point.waypoint.pose.linear()).angle();
+
+        return (dp <= params.distance.linear) and (dr <= params.distance.angular);
+    }
+
+private:
+    core::TrajectoryPoint point;
+    const Params params;
+};
+
+/**
+ * @brief Tests the control
+ *
+ */
+BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerReachGoal)
+{
+    core::Scene scene;
+    scene.robot = std::make_shared<VirtualRobot::LocalRobot>("foo", "bar");
+    scene.robot->setGlobalPose(Eigen::Matrix4f::Identity() *
+                               core::Pose(Eigen::Translation3f(0, 100, 0)).matrix(),
+                               false);
+
+    traj_ctrl::TrajectoryFollowingControllerParams params;
+    params.pidPos = {1, 0, 0};
+    params.pidOri = {1, 0, 0};
+    params.limits.linearVelocity = std::numeric_limits<float>::max();
+    params.limits.angularVelocity = std::numeric_limits<float>::max();
+
+    traj_ctrl::TrajectoryFollowingController controller(params, scene);
+
+    core::TrajectoryPoint wpBefore;
+    wpBefore.waypoint.pose = core::Pose::Identity();
+    wpBefore.waypoint.pose.translation().x() -= 100.F;
+    wpBefore.twist.linear = Eigen::Vector3f::UnitX();
+    wpBefore.twist.angular = Eigen::Vector3f::Zero();
+
+    core::TrajectoryPoint wpAfter = wpBefore;
+    wpAfter.waypoint.pose.translation().x() += 2000.F;
+
+    core::TrajectoryPtr traj(new core::Trajectory({wpBefore, wpAfter}));
+
+    auto timestamp = std::chrono::steady_clock::now();
+
+    TargetReachedCondition condition(wpAfter,
+    TargetReachedCondition::Params{.distance{
+            .linear = 50.F, .angular = simox::math::deg_to_rad(10.F)}});
+
+    for (int i = 0; i < 50; i++)
+    {
+        core::Twist controlVal = controller.control(traj);
+        ARMARX_DEBUG << "Control val " << controlVal.linear;
+
+        auto now = std::chrono::steady_clock::now();
+
+        const float dt = std::chrono::duration<float>(now - timestamp).count();
+
+        core::Pose dp = controlVal.poseDiff(dt);
+
+        const core::Pose newPose = core::Pose(scene.robot->getGlobalPose()) * dp;
+        scene.robot->setGlobalPose(newPose.matrix());
+
+        if (condition.check(newPose))
+        {
+            break;
+        }
+
+
+    }
+
+    BOOST_CHECK(condition.check(core::Pose(scene.robot->getGlobalPose())));
+}