diff --git a/source/Navigation/libraries/core/CMakeLists.txt b/source/Navigation/libraries/core/CMakeLists.txt
index 44373e94c8c8d2c050bb9caa90bce821daf8ed71..c94aa639a276ffaca920f1e60254f1e3488b4422 100644
--- a/source/Navigation/libraries/core/CMakeLists.txt
+++ b/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
diff --git a/source/Navigation/libraries/core/types.cpp b/source/Navigation/libraries/core/types.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a8e5e149b4caed0957cd6129ec0f14747c3f0cfe
--- /dev/null
+++ b/source/Navigation/libraries/core/types.cpp
@@ -0,0 +1,10 @@
+#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
diff --git a/source/Navigation/libraries/core/types.h b/source/Navigation/libraries/core/types.h
index b00682034dd561e7edce9141fb8303c04aedf523..6b820ea7af87deb15f4e47e6e2decacc5ab715dd 100644
--- a/source/Navigation/libraries/core/types.h
+++ b/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;
diff --git a/source/Navigation/libraries/global_planning/Point2Point.cpp b/source/Navigation/libraries/global_planning/Point2Point.cpp
index 0fcfdc513e87ab2afc8b76f93c5b3ca639d2013f..13407ff3e4af30f52d548bd024b2dd34fd68bdef 100644
--- a/source/Navigation/libraries/global_planning/Point2Point.cpp
+++ b/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()}});
}
diff --git a/source/Navigation/libraries/server/Navigator.cpp b/source/Navigation/libraries/server/Navigator.cpp
index 7b77b759c05264ac6f060d0aebaade02ae666070..927792ff38fa8c568bb7c3a6805a77c26b197efc 100644
--- a/source/Navigation/libraries/server/Navigator.cpp
+++ b/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)
{
diff --git a/source/Navigation/libraries/server/execution/DummyExecutor.h b/source/Navigation/libraries/server/execution/DummyExecutor.h
index d5d075e58910c4c1dd358bbf9af8fa77765cd269..c5bf0cb99fa1e70f46f9323eef3d8df1fbaf1842 100644
--- a/source/Navigation/libraries/server/execution/DummyExecutor.h
+++ b/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());
}
diff --git a/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp b/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp
index a9cda01677ac16992f8250a3646216eaba8ef519..a6e8743c660791ef8159ba47189321cb5264cb31 100644
--- a/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp
+++ b/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())));
+}