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)
|
||||
robot_trajectory->reverse();
|
||||
solution.setTrajectory(robot_trajectory);
|
||||
|
||||
// set cost
|
||||
double cost = 0;
|
||||
for (const double& distance : robot_trajectory->getWayPointDurations()) {
|
||||
cost += distance;
|
||||
}
|
||||
solution.setCost(cost);
|
||||
|
||||
if (!success)
|
||||
solution.markAsFailure();
|
||||
return true;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user