diff --git a/source/Navigation/libraries/core/Trajectory.cpp b/source/Navigation/libraries/core/Trajectory.cpp
index 714a84efda15ec385f6ad4f01d78f8d0de3d4ced..d513509a30f66bb1ce853ca4c9345b1db3f1d4f1 100644
--- a/source/Navigation/libraries/core/Trajectory.cpp
+++ b/source/Navigation/libraries/core/Trajectory.cpp
@@ -9,7 +9,7 @@
namespace armarx::nav::core
{
- Waypoint Trajectory::getProjection(const Pose& pose) const
+ Point Trajectory::getProjection(const Pose& pose) const
{
const auto asPt = [](const Pose & pose) -> wykobi::point2d<float>
{ return wykobi::make_point<float>(pose.translation().x(), pose.translation().y()); };
@@ -18,14 +18,14 @@ namespace armarx::nav::core
float distance = std::numeric_limits<float>::max();
- Waypoint bestProj;
+ Point bestProj;
- for (size_t i = 1; i < size(); i++)
+ for (size_t i = 1; i < points.size(); i++)
{
- const auto& wpBefore = at(i);
- const auto& wpAfter = at(i + 1);
+ const auto& wpBefore = points.at(i);
+ const auto& wpAfter = points.at(i + 1);
- auto segment = wykobi::make_segment(asPt(wpBefore.pose), asPt(wpAfter.pose));
+ auto segment = wykobi::make_segment(asPt(wpBefore.waypoint.pose), asPt(wpAfter.waypoint.pose));
wykobi::point2d<float> closest_point =
wykobi::closest_point_on_segment_from_point(segment, point);
@@ -34,18 +34,15 @@ namespace armarx::nav::core
if (currentDistance < distance)
{
- const float d1 = wykobi::distance(closest_point, asPt(wpBefore.pose));
- const float d = wykobi::distance(asPt(wpBefore.pose), asPt(wpAfter.pose));
+ const float d1 = wykobi::distance(closest_point, asPt(wpBefore.waypoint.pose));
+ const float d = wykobi::distance(asPt(wpBefore.waypoint.pose), asPt(wpAfter.waypoint.pose));
const float t = d1 / d;
- math::LinearInterpolatedPose ip(wpBefore.pose.matrix(), wpAfter.pose.matrix(), 0, 1, true);
- bestProj.pose = ip.Get(t);
-
- if (wpAfter.twist.has_value() and wpBefore.twist.has_value())
- {
- bestProj.twist = (*wpAfter.twist - *wpBefore.twist) * t;
- }
+ 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;
distance = currentDistance;
}
diff --git a/source/Navigation/libraries/trajectory_control/CMakeLists.txt b/source/Navigation/libraries/trajectory_control/CMakeLists.txt
index 3a8744d1617fa510312c68b50b7c83ac967c9f75..e71d5280068757ddb1c122bce0fdb8571f54c701 100644
--- a/source/Navigation/libraries/trajectory_control/CMakeLists.txt
+++ b/source/Navigation/libraries/trajectory_control/CMakeLists.txt
@@ -7,6 +7,7 @@ armarx_add_library(
LIBS
ArmarXCoreInterfaces
ArmarXCore
+ RobotAPICore
Navigation::core
SOURCES
diff --git a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
index 25f184414eefbffc1cb91b95f3fc4dafb76154aa..9a10519fe11c89202f5aa4538fda96029cfe94e0 100644
--- a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
@@ -36,7 +36,7 @@ namespace armarx::nav::traj_ctrl
}
TrajectoryFollowingController::TrajectoryFollowingController(const Params& params,
- const core::Scene& context) :
+ const core::Scene& context) :
TrajectoryController(context),
params(params),
pidPos(params.pidPos.Kp,
@@ -45,14 +45,17 @@ namespace armarx::nav::traj_ctrl
std::numeric_limits<double>::max(),
std::numeric_limits<double>::max(),
false,
- std::vector<bool>{false, false, false}),
- pidOri(params.pidOri.Kp,
- params.pidOri.Ki,
- params.pidOri.Kd,
- std::numeric_limits<double>::max(),
- std::numeric_limits<double>::max(),
- false,
- std::vector<bool>{true, true, true})
+ std::vector<bool>
+ {
+ false, false, false
+ }),
+ pidOri(params.pidOri.Kp,
+ params.pidOri.Ki,
+ params.pidOri.Kd,
+ std::numeric_limits<double>::max(),
+ std::numeric_limits<double>::max(),
+ false,
+ std::vector<bool> {true, true, true})
{
}
@@ -76,7 +79,7 @@ namespace armarx::nav::traj_ctrl
// scale such that no limit is violated
twist.linear /= scaleMax;
twist.angular /= scaleMax;
-
+
return twist;
}
@@ -90,13 +93,13 @@ namespace armarx::nav::traj_ctrl
ARMARX_INFO << targetPose.waypoint.pose.translation();
- const auto asXYZRPY = [](const core::Pose& pose)
- {
- Eigen::Vector6f p;
- p.head<3>() = pose.translation();
- p.bottomRows(3) = simox::math::mat4f_to_rpy(pose.matrix());
- return p;
- };
+ // const auto asXYZRPY = [](const core::Pose & pose)
+ // {
+ // Eigen::Vector6f p;
+ // p.head<3>() = pose.translation();
+ // p.bottomRows(3) = simox::math::mat4f_to_rpy(pose.matrix());
+ // return p;
+ // };
using simox::math::mat4f_to_rpy;
using simox::math::mat4f_to_pos;
@@ -105,11 +108,11 @@ namespace armarx::nav::traj_ctrl
pidOri.update(mat4f_to_rpy(currentPose.matrix()), mat4f_to_rpy(targetPose.waypoint.pose.matrix()));
// TODO: better handling if feed forward velocity is not set. need movement direction ...
- core::Twist twist =
+ core::Twist twist =
{
- pidPos.getControlValue(), //+ *targetPose.twist;
- pidOri.getControlValue() //+ *targetPose.twist;
- }
+ .linear = pidPos.getControlValue(), //+ *targetPose.twist;
+ .angular = pidOri.getControlValue() //+ *targetPose.twist;
+ };
return applyTwistLimits(twist);
}
diff --git a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h
index c8f28f93762b724feac3785d8230d9ed67707f3b..81943af94e96afb67a7392b33330a2f3cdc7ef87 100644
--- a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h
+++ b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.h
@@ -21,6 +21,8 @@
#pragma once
+#include "RobotAPI/libraries/core/MultiDimPIDController.h"
+
#include "Navigation/libraries/trajectory_control/TrajectoryController.h"
namespace armarx::nav::traj_ctrl