update

source/Navigation/libraries/core/Trajectory.cpp

@@ -9,7 +9,9 @@
 
 namespace armarx::nav::core
 {
-    TrajectoryPoint Trajectory::getProjection(const Pose& pose) const
+    TrajectoryPoint
+    Trajectory::getProjection(const Pose& pose,
+                              const VelocityInterpolations& velocityInterpolation) const
     {
         const auto asPt = [](const Pose & pose) -> wykobi::point2d<float>
         { return wykobi::make_point<float>(pose.translation().x(), pose.translation().y()); };
@@ -20,12 +22,13 @@ namespace armarx::nav::core
 
         TrajectoryPoint bestProj;
 
-        for (size_t i = 0; i < points.size() -1; i++)
+        for (size_t i = 0; i < points.size() - 1; i++)
         {
             const auto& wpBefore = points.at(i);
             const auto& wpAfter = points.at(i + 1);
 
-            auto segment = wykobi::make_segment(asPt(wpBefore.waypoint.pose), asPt(wpAfter.waypoint.pose));
+            auto segment =
+                wykobi::make_segment(asPt(wpBefore.waypoint.pose), asPt(wpAfter.waypoint.pose));
 
             wykobi::point2d<float> closestPoint =
                 wykobi::closest_point_on_segment_from_point(segment, point);
@@ -35,14 +38,25 @@ namespace armarx::nav::core
             {
 
                 const float d1 = wykobi::distance(closestPoint, asPt(wpBefore.waypoint.pose));
-                const float d = wykobi::distance(asPt(wpBefore.waypoint.pose), asPt(wpAfter.waypoint.pose));
+                const float d =
+                    wykobi::distance(asPt(wpBefore.waypoint.pose), asPt(wpAfter.waypoint.pose));
 
                 const float t = d1 / d;
 
-                math::LinearInterpolatedPose ip(wpBefore.waypoint.pose.matrix(), wpAfter.waypoint.pose.matrix(), 0, 1, true);
+                math::LinearInterpolatedPose ip(
+                    wpBefore.waypoint.pose.matrix(), wpAfter.waypoint.pose.matrix(), 0, 1, true);
                 bestProj.waypoint.pose = ip.Get(t);
-                bestProj.twist.linear = (wpAfter.twist.linear - wpBefore.twist.linear) * t;
-                bestProj.twist.angular = (wpAfter.twist.angular - wpBefore.twist.angular) * t;
+
+                switch (velocityInterpolation)
+                {
+                    case VelocityInterpolations::LinearInterpolation:
+                        bestProj.twist.linear = (wpAfter.twist.linear - wpBefore.twist.linear) * t;
+                        bestProj.twist.angular =
+                            (wpAfter.twist.angular - wpBefore.twist.angular) * t;
+                        break;
+                    case VelocityInterpolations::LastWaypoint:
+                        bestProj.twist = wpBefore.twist;
+                }
 
                 distance = currentDistance;
             }

source/Navigation/libraries/core/Trajectory.h

@@ -32,6 +32,12 @@ namespace armarx::nav::core
         Twist twist;
     };
 
+    enum class VelocityInterpolations
+    {
+        LinearInterpolation,
+        LastWaypoint
+    };
+
     class Trajectory
     {
     public:
@@ -39,7 +45,7 @@ namespace armarx::nav::core
         {
         }
 
-        TrajectoryPoint getProjection(const Pose& pose) const;
+        TrajectoryPoint getProjection(const Pose& pose, const VelocityInterpolations& velocityInterpolation) const;
 
     private:
         std::vector<TrajectoryPoint> points;

source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp

@@ -89,7 +89,7 @@ namespace armarx::nav::traj_ctrl
 
         const core::Pose currentPose(context.robot->getGlobalPose());
 
-        const auto targetPose = trajectory->getProjection(currentPose);
+        const auto targetPose = trajectory->getProjection(currentPose, core::VelocityInterpolations::LastWaypoint);
 
         ARMARX_INFO << targetPose.waypoint.pose.translation();
 
@@ -107,6 +107,9 @@ namespace armarx::nav::traj_ctrl
         pidPos.update(mat4f_to_pos(currentPose.matrix()), mat4f_to_pos(targetPose.waypoint.pose.matrix()));
         pidOri.update(mat4f_to_rpy(currentPose.matrix()), mat4f_to_rpy(targetPose.waypoint.pose.matrix()));
 
+        ARMARX_INFO << "Feed forward velocity " << targetPose.twist.linear;
+        ARMARX_INFO << "Control value " << pidPos.getControlValue();
+
         // TODO: better handling if feed forward velocity is not set. need movement direction ...
         core::Twist twist =
         {

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

@@ -20,8 +20,14 @@
  *             GNU General Public License
  */
 
+#include <limits>
+
 #include <Eigen/Geometry>
+#include <Eigen/src/Geometry/Transform.h>
+#include <Eigen/src/Geometry/Translation.h>
+
 #include <VirtualRobot/Robot.h>
+
 #include "Navigation/libraries/core/Trajectory.h"
 #define BOOST_TEST_MODULE Navigation::ArmarXLibraries::trajectory_control
 
@@ -37,29 +43,76 @@
 
 using namespace armarx::nav;
 
-BOOST_AUTO_TEST_CASE(testTrajectoryFollowingController)
+BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerOnTrajectory)
 {
 
     core::Scene scene;
     scene.robot = std::make_shared<VirtualRobot::LocalRobot>("foo", "bar");
     scene.robot->setGlobalPose(Eigen::Matrix4f::Identity(), false);
 
-    traj_ctrl::TrajectoryFollowingController controller(traj_ctrl::TrajectoryFollowingControllerParams(),
-                                             scene);
+    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 = Eigen::Affine3f::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}));
+
+    core::Twist controlVal = controller.control(traj);
+
+    ARMARX_DEBUG << "Control val " << controlVal.linear;
+
+    // only feed-forward velocity needs to be considered
+    BOOST_CHECK_EQUAL(controlVal.linear.x(), 1.0);
+
+    BOOST_CHECK_EQUAL(true, true);
+}
+
+
+/**
+ * @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);
+
+    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 = Eigen::Affine3f::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}));
 
     core::Twist controlVal = controller.control(traj);
 
+    ARMARX_DEBUG << "Control val " << controlVal.linear;
+
     // only feed-forward velocity needs to be considered
     BOOST_CHECK_EQUAL(controlVal.linear.x(), 1.0);