From b35aba6e1935c35be72abbdf8f2af9bb08acbc0f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 26 May 2018 20:11:47 +0200 Subject: [PATCH] validate merged trajectories --- .../moveit/task_constructor/container_p.h | 6 ++--- core/src/container.cpp | 22 +++++++++---------- core/src/stages/connect.cpp | 4 +++- 3 files changed, 17 insertions(+), 15 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index ebd23ebe..1fc9353d 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -245,12 +245,12 @@ public: void onNewPropagateSolution(const SolutionBase& s); void onNewGeneratorSolution(const SolutionBase& s); void mergeAnyCombination(const ChildSolutionMap& all_solutions, const SolutionBase& current, - const moveit::core::RobotState& state, const Spawner& spawner); - void merge(const ChildSolutionList& sub_solutions, const moveit::core::RobotState& state, const Spawner& spawner); + const planning_scene::PlanningSceneConstPtr& start_scene, const Spawner& spawner); + void merge(const ChildSolutionList& sub_solutions, + const planning_scene::PlanningSceneConstPtr& start_scene, const Spawner& spawner); void sendForward(SubTrajectory&& t, const InterfaceState* from); void sendBackward(SubTrajectory&& t, const InterfaceState* to); - void spawn(SubTrajectory&& t); }; PIMPL_FUNCTIONS(Merger) diff --git a/core/src/container.cpp b/core/src/container.cpp index 22f1d60d..0b08f0c8 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -1031,7 +1031,7 @@ void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) // combine the new solution with all solutions from other children auto spawner = dir == PROPAGATE_FORWARDS ? &MergerPrivate::sendForward : &MergerPrivate::sendBackward; - mergeAnyCombination(all_solutions, s, external_source_state->scene()->getCurrentState(), + mergeAnyCombination(all_solutions, s, external_source_state->scene(), std::bind(spawner, this, std::placeholders::_1, external_source_state)); } @@ -1053,16 +1053,11 @@ void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to) void MergerPrivate::onNewGeneratorSolution(const SolutionBase& s) { - // TODO -} - -void MergerPrivate::spawn(SubTrajectory&& t) -{ - + // TODO: implement in similar fashion as onNewPropagateSolution(), but also merge start/end states } void MergerPrivate::mergeAnyCombination(const ChildSolutionMap& all_solutions, const SolutionBase& current, - const moveit::core::RobotState& state, const Spawner& spawner) + const planning_scene::PlanningSceneConstPtr& start_scene, const Spawner& spawner) { std::vector indeces; // which solution index was considered last for i-th child? indeces.reserve(children().size()); @@ -1077,7 +1072,7 @@ void MergerPrivate::mergeAnyCombination(const ChildSolutionMap& all_solutions, c sub_solutions.push_back(pair.second[indeces.back()]); } while (true) { - merge(sub_solutions, state, spawner); + merge(sub_solutions, start_scene, spawner); // compose next combination size_t child = 0; @@ -1097,7 +1092,8 @@ void MergerPrivate::mergeAnyCombination(const ChildSolutionMap& all_solutions, c } } -void MergerPrivate::merge(const ChildSolutionList& sub_solutions, const moveit::core::RobotState& state, const Spawner& spawner) +void MergerPrivate::merge(const ChildSolutionList& sub_solutions, + const planning_scene::PlanningSceneConstPtr& start_scene, const Spawner& spawner) { // transform vector of SubTrajectories into vector of RobotTrajectories std::vector sub_trajectories; @@ -1110,11 +1106,15 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions, const moveit:: } moveit::core::JointModelGroup *jmg = jmg_merged_.get(); - robot_trajectory::RobotTrajectoryPtr merged = task_constructor::merge(sub_trajectories, state, jmg); + robot_trajectory::RobotTrajectoryPtr merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg); if (jmg_merged_.get() != jmg) jmg_merged_.reset(jmg); if (!merged) return; + // check merged trajectory for collisions + if (!start_scene->isPathValid(*merged)) + return; + SubTrajectory t(merged); // accumulate costs and markers double costs = 0.0; diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index d1fc2f12..68a68939 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -227,7 +227,9 @@ SubTrajectoryPtr Connect::merge(const std::vectorisPathValid(*trajectory, properties().get("path_constraints"))) + return SubTrajectoryPtr(); return std::make_shared(trajectory); }