mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
a92753dafc
commit
6dbc742f20
@ -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
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user