mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
adding cost calculations to connect and move_to
This commit is contained in:
parent
1783587f41
commit
7ca49bc29b
@ -190,6 +190,13 @@ SolutionSequencePtr Connect::makeSequential(const std::vector<robot_trajectory::
|
||||
planning_scene::PlanningSceneConstPtr start_ps = *scene_it;
|
||||
const InterfaceState* state = &from;
|
||||
|
||||
// calculate cost
|
||||
double cost = 0;
|
||||
for (const auto& trajectory: sub_trajectories) {
|
||||
for (const double& distance: trajectory->getWayPointDurations())
|
||||
cost += distance;
|
||||
}
|
||||
|
||||
SolutionSequence::container_type sub_solutions;
|
||||
for (const auto &sub : sub_trajectories) {
|
||||
planning_scene::PlanningSceneConstPtr end_ps = *++scene_it;
|
||||
@ -212,16 +219,23 @@ SolutionSequencePtr Connect::makeSequential(const std::vector<robot_trajectory::
|
||||
start_ps = end_ps;
|
||||
}
|
||||
|
||||
return std::make_shared<SolutionSequence>(std::move(sub_solutions));
|
||||
return std::make_shared<SolutionSequence>(std::move(sub_solutions), cost);
|
||||
}
|
||||
|
||||
SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
||||
const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
|
||||
const moveit::core::RobotState& state)
|
||||
{
|
||||
// calculate cost
|
||||
double cost = 0;
|
||||
for (const auto& trajectory: sub_trajectories) {
|
||||
for (const double& distance: trajectory->getWayPointDurations())
|
||||
cost += distance;
|
||||
}
|
||||
|
||||
// no need to merge if there is only a single sub trajectory
|
||||
if (sub_trajectories.size() == 1)
|
||||
return std::make_shared<SubTrajectory>(sub_trajectories[0]);
|
||||
return std::make_shared<SubTrajectory>(sub_trajectories[0], cost);
|
||||
|
||||
auto jmg = merged_jmg_.get();
|
||||
assert(jmg);
|
||||
@ -233,7 +247,7 @@ SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTraject
|
||||
if (!intermediate_scenes.front()->isPathValid(*trajectory, properties().get<moveit_msgs::Constraints>("path_constraints")))
|
||||
return SubTrajectoryPtr();
|
||||
|
||||
return std::make_shared<SubTrajectory>(trajectory);
|
||||
return std::make_shared<SubTrajectory>(trajectory, cost);
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
@ -234,11 +234,18 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
|
||||
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
||||
}
|
||||
|
||||
// set cost
|
||||
double cost = 0;
|
||||
for (const double& distance: robot_trajectory->getWayPointDurations()) {
|
||||
cost += distance;
|
||||
}
|
||||
|
||||
// store result
|
||||
if (robot_trajectory) {
|
||||
scene->setCurrentState(robot_trajectory->getLastWayPoint());
|
||||
if (dir == BACKWARD) robot_trajectory->reverse();
|
||||
solution.setTrajectory(robot_trajectory);
|
||||
solution.setCost(cost);
|
||||
if (!success)
|
||||
solution.markAsFailure();
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user