From 6dbc742f2071cff78bf1a054f05bf5edd41b5b53 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 17:01:24 +0200 Subject: [PATCH] implement correct PathLengthCost The previous implementation depends on the dynamics limits of the robot, which might be interesting in some cases, but shouldn't be a default anywhere. --- core/include/moveit/task_constructor/cost_terms.h | 5 ++++- core/src/cost_terms.cpp | 12 ++++++++++++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index c32c192c..5a6bc8ed 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -18,9 +18,12 @@ public: double cost; }; -/// execution duration of the whole trajectory +/// trajectory length (interpolated between waypoints) double PathLengthCost(const SubTrajectory& s); +/// execution duration of the whole trajectory +double TrajectoryDurationCost(const SubTrajectory& s); + /** inverse distance to collision * * \arg interface compute distances using START or END interface of solution diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index e7edd2e2..49a16d1f 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -49,6 +49,18 @@ namespace task_constructor { namespace cost { double PathLengthCost(const SubTrajectory& s) { + const auto& traj = s.trajectory(); + + if (traj == nullptr) + return 0.0; + + double path_length{ 0.0 }; + for (size_t i = 1; i < traj->getWayPointCount(); ++i) + path_length += traj->getWayPoint(i - 1).distance(traj->getWayPoint(i)); + return path_length; +} + +double TrajectoryDurationCost(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; }