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.
This commit is contained in:
v4hn 2020-06-30 17:01:24 +02:00
parent a92753dafc
commit 6dbc742f20
2 changed files with 16 additions and 1 deletions

View File

@ -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

View File

@ -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;
}