diff --git a/source/Navigation/libraries/core/Trajectory.cpp b/source/Navigation/libraries/core/Trajectory.cpp
index 3f463a004c469746ed07df7f8424bcdbc55867b3..469c511ffcd762f27bd0d91d6a4efe7cdb2e165c 100644
--- a/source/Navigation/libraries/core/Trajectory.cpp
+++ b/source/Navigation/libraries/core/Trajectory.cpp
@@ -9,7 +9,7 @@
namespace armarx::nav::core
{
- TrajectoryPoint
+ Projection
Trajectory::getProjection(const Pose& pose,
const VelocityInterpolations& velocityInterpolation) const
{
@@ -20,7 +20,7 @@ namespace armarx::nav::core
float distance = std::numeric_limits<float>::max();
- TrajectoryPoint bestProj;
+ Projection bestProj;
for (size_t i = 0; i < points.size() - 1; i++)
{
@@ -45,17 +45,20 @@ namespace armarx::nav::core
math::LinearInterpolatedPose ip(
wpBefore.waypoint.pose.matrix(), wpAfter.waypoint.pose.matrix(), 0, 1, true);
- bestProj.waypoint.pose = ip.Get(t);
+
+ bestProj.wayPointBefore = wpBefore;
+ bestProj.wayPointAfter = wpAfter;
+ bestProj.projection.waypoint.pose = ip.Get(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;
+ bestProj.projection.velocity = (wpAfter.velocity - wpBefore.velocity) * t;
+ bestProj.projection.velocity =
+ (wpAfter.velocity - wpBefore.velocity) * t;
break;
case VelocityInterpolations::LastWaypoint:
- bestProj.twist = wpBefore.twist;
+ bestProj.projection.velocity = wpBefore.velocity;
}
distance = currentDistance;
diff --git a/source/Navigation/libraries/core/Trajectory.h b/source/Navigation/libraries/core/Trajectory.h
index 5f2a1c07b2cee954affcbe137b9296065702cd13..64beb80492072a34f8e931ab9c7ff3c2f792cf48 100644
--- a/source/Navigation/libraries/core/Trajectory.h
+++ b/source/Navigation/libraries/core/Trajectory.h
@@ -29,7 +29,15 @@ namespace armarx::nav::core
struct TrajectoryPoint
{
Waypoint waypoint;
- Twist twist;
+ float velocity;
+ };
+
+ struct Projection
+ {
+ TrajectoryPoint projection;
+
+ TrajectoryPoint wayPointBefore;
+ TrajectoryPoint wayPointAfter;
};
enum class VelocityInterpolations
@@ -45,7 +53,8 @@ namespace armarx::nav::core
{
}
- TrajectoryPoint getProjection(const Pose& pose, const VelocityInterpolations& velocityInterpolation) const;
+ Projection getProjection(const Pose& pose,
+ const VelocityInterpolations& velocityInterpolation) const;
private:
std::vector<TrajectoryPoint> points;
diff --git a/source/Navigation/libraries/global_planning/Point2Point.cpp b/source/Navigation/libraries/global_planning/Point2Point.cpp
index fef24e7f71f9d3fadc737ad00769cbb04c1d12a1..7db0e0544227a6b0135311ce487e974f60e44e39 100644
--- a/source/Navigation/libraries/global_planning/Point2Point.cpp
+++ b/source/Navigation/libraries/global_planning/Point2Point.cpp
@@ -62,13 +62,13 @@ namespace armarx::nav::glob_plan
trajectory.push_back(core::TrajectoryPoint
{
.waypoint = core::Waypoint{.pose = core::Pose(scene.robot->getGlobalPose())},
- .twist = {.linear = Eigen::Vector3f::Zero(), .angular = Eigen::Vector3f::Zero()}});
+ .velocity = params.velocity});
}
trajectory.push_back(core::TrajectoryPoint
{
.waypoint = core::Waypoint{.pose = goal},
- .twist = {.linear = Eigen::Vector3f::Zero(), .angular = Eigen::Vector3f::Zero()}});
+ .velocity = params.velocity});
return std::make_shared<core::Trajectory>(trajectory);
}
diff --git a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
index 17174964573ae8847e22e9a0867d0f11cc6cbd75..1028b28d0bb3d55cb2af16ac1775a1cbd0239852 100644
--- a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
+++ b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
@@ -102,10 +102,10 @@ namespace armarx::nav::traj_ctrl
const core::Pose currentPose(context.robot->getGlobalPose());
- const auto targetPose =
+ const auto projectedPose =
trajectory->getProjection(currentPose, core::VelocityInterpolations::LastWaypoint);
- ARMARX_INFO << targetPose.waypoint.pose.translation();
+ ARMARX_INFO << projectedPose.projection.waypoint.pose.translation();
// const auto asXYZRPY = [](const core::Pose & pose)
// {
@@ -119,16 +119,22 @@ namespace armarx::nav::traj_ctrl
using simox::math::mat4f_to_rpy;
pidPos.update(mat4f_to_pos(currentPose.matrix()),
- mat4f_to_pos(targetPose.waypoint.pose.matrix()));
+ mat4f_to_pos(projectedPose.projection.waypoint.pose.matrix()));
pidOri.update(mat4f_to_rpy(currentPose.matrix()),
- mat4f_to_rpy(targetPose.waypoint.pose.matrix()));
+ mat4f_to_rpy(projectedPose.projection.waypoint.pose.matrix()));
- ARMARX_INFO << "Feed forward velocity " << targetPose.twist.linear;
+ const Eigen::Vector3f feedforwardVelocity =
+ (projectedPose.wayPointAfter.waypoint.pose.translation() -
+ projectedPose.wayPointBefore.waypoint.pose.translation())
+ .normalized() *
+ projectedPose.projection.velocity;
+
+ ARMARX_INFO << "Feed forward velocity " << feedforwardVelocity;
ARMARX_INFO << "Control value " << pidPos.getControlValue();
// TODO: better handling if feed forward velocity is not set. need movement direction ...
- core::Twist twist = {.linear = pidPos.getControlValue() + targetPose.twist.linear,
- .angular = pidOri.getControlValue() + targetPose.twist.angular
+ core::Twist twist = {.linear = pidPos.getControlValue() + feedforwardVelocity,
+ .angular = pidOri.getControlValue()
};
return applyTwistLimits(twist);
diff --git a/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp b/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp
index 40a73bd90266c81cebd71826e4e81b1869d02402..34f77cfda0ddd946243fa68228dc693a61444c0d 100644
--- a/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp
+++ b/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp
@@ -64,8 +64,7 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerOnTrajectory)
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();
+ wpBefore.velocity = 100.0;
core::TrajectoryPoint wpAfter = wpBefore;
wpAfter.waypoint.pose.translation().x() += 2000.F;
@@ -104,8 +103,7 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerNextToTrajectory)
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();
+ wpBefore.velocity = 100.0;
core::TrajectoryPoint wpAfter = wpBefore;
wpAfter.waypoint.pose.translation().x() += 2000.F;
@@ -153,9 +151,8 @@ private:
const Params params;
};
-class TimeAwareRobot: virtual public VirtualRobot::LocalRobot
+class TimeAwareRobot : virtual public VirtualRobot::LocalRobot
{
-
};
/**
@@ -180,8 +177,7 @@ BOOST_AUTO_TEST_CASE(testTrajectoryFollowingControllerReachGoal)
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();
+ wpBefore.velocity = 100.0;
core::TrajectoryPoint wpAfter = wpBefore;
wpAfter.waypoint.pose.translation().x() += 2000.F;