mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Added cost calculation in move_relative (#108)
This commit is contained in:
parent
ff297d2929
commit
145bec1ed3
@ -288,6 +288,14 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
|||||||
if (dir == BACKWARD)
|
if (dir == BACKWARD)
|
||||||
robot_trajectory->reverse();
|
robot_trajectory->reverse();
|
||||||
solution.setTrajectory(robot_trajectory);
|
solution.setTrajectory(robot_trajectory);
|
||||||
|
|
||||||
|
// set cost
|
||||||
|
double cost = 0;
|
||||||
|
for (const double& distance : robot_trajectory->getWayPointDurations()) {
|
||||||
|
cost += distance;
|
||||||
|
}
|
||||||
|
solution.setCost(cost);
|
||||||
|
|
||||||
if (!success)
|
if (!success)
|
||||||
solution.markAsFailure();
|
solution.markAsFailure();
|
||||||
return true;
|
return true;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user