From d24d41337a8599c8e68742f998e858682d45549c Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 8 Jul 2021 16:34:05 +0200
Subject: [PATCH] update
---
.../Navigation/libraries/core/Trajectory.cpp | 28 ++++++---
source/Navigation/libraries/core/Trajectory.h | 8 ++-
.../TrajectoryFollowingController.cpp | 5 +-
.../test/trajectory_controlTest.cpp | 59 ++++++++++++++++++-
4 files changed, 88 insertions(+), 12 deletions(-)
diff --git a/source/Navigation/libraries/core/Trajectory.cpp b/source/Navigation/libraries/core/Trajectory.cpp
index c673e059..3f463a00 100644
--- a/source/Navigation/libraries/core/Trajectory.cpp
+++ b/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;
}
diff --git a/source/Navigation/libraries/core/Trajectory.h b/source/Navigation/libraries/core/Trajectory.h
index 9a9b43a6..5f2a1c07 100644
--- a/source/Navigation/libraries/core/Trajectory.h
+++ b/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;
diff --git a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp b/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
index 34330e99..eda53b12 100644
--- a/source/Navigation/libraries/trajectory_control/TrajectoryFollowingController.cpp
+++ b/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 =
{
diff --git a/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp b/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp
index 4b30e427..a9cda016 100644
--- a/source/Navigation/libraries/trajectory_control/test/trajectory_controlTest.cpp
+++ b/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);
--
GitLab