mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
validate merged trajectories
This commit is contained in:
parent
941e9df737
commit
b35aba6e19
@ -245,12 +245,12 @@ public:
|
|||||||
void onNewPropagateSolution(const SolutionBase& s);
|
void onNewPropagateSolution(const SolutionBase& s);
|
||||||
void onNewGeneratorSolution(const SolutionBase& s);
|
void onNewGeneratorSolution(const SolutionBase& s);
|
||||||
void mergeAnyCombination(const ChildSolutionMap& all_solutions, const SolutionBase& current,
|
void 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);
|
||||||
void merge(const ChildSolutionList& sub_solutions, const moveit::core::RobotState& state, 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 sendForward(SubTrajectory&& t, const InterfaceState* from);
|
||||||
void sendBackward(SubTrajectory&& t, const InterfaceState* to);
|
void sendBackward(SubTrajectory&& t, const InterfaceState* to);
|
||||||
void spawn(SubTrajectory&& t);
|
|
||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(Merger)
|
PIMPL_FUNCTIONS(Merger)
|
||||||
|
|
||||||
|
|||||||
@ -1031,7 +1031,7 @@ void MergerPrivate::onNewPropagateSolution(const SolutionBase& s)
|
|||||||
|
|
||||||
// combine the new solution with all solutions from other children
|
// combine the new solution with all solutions from other children
|
||||||
auto spawner = dir == PROPAGATE_FORWARDS ? &MergerPrivate::sendForward : &MergerPrivate::sendBackward;
|
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));
|
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)
|
void MergerPrivate::onNewGeneratorSolution(const SolutionBase& s)
|
||||||
{
|
{
|
||||||
// TODO
|
// TODO: implement in similar fashion as onNewPropagateSolution(), but also merge start/end states
|
||||||
}
|
|
||||||
|
|
||||||
void MergerPrivate::spawn(SubTrajectory&& t)
|
|
||||||
{
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void MergerPrivate::mergeAnyCombination(const ChildSolutionMap& all_solutions, const SolutionBase& current,
|
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<size_t> indeces; // which solution index was considered last for i-th child?
|
std::vector<size_t> indeces; // which solution index was considered last for i-th child?
|
||||||
indeces.reserve(children().size());
|
indeces.reserve(children().size());
|
||||||
@ -1077,7 +1072,7 @@ void MergerPrivate::mergeAnyCombination(const ChildSolutionMap& all_solutions, c
|
|||||||
sub_solutions.push_back(pair.second[indeces.back()]);
|
sub_solutions.push_back(pair.second[indeces.back()]);
|
||||||
}
|
}
|
||||||
while (true) {
|
while (true) {
|
||||||
merge(sub_solutions, state, spawner);
|
merge(sub_solutions, start_scene, spawner);
|
||||||
|
|
||||||
// compose next combination
|
// compose next combination
|
||||||
size_t child = 0;
|
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
|
// transform vector of SubTrajectories into vector of RobotTrajectories
|
||||||
std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories;
|
std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories;
|
||||||
@ -1110,11 +1106,15 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions, const moveit::
|
|||||||
}
|
}
|
||||||
|
|
||||||
moveit::core::JointModelGroup *jmg = jmg_merged_.get();
|
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)
|
if (jmg_merged_.get() != jmg)
|
||||||
jmg_merged_.reset(jmg);
|
jmg_merged_.reset(jmg);
|
||||||
if (!merged) return;
|
if (!merged) return;
|
||||||
|
|
||||||
|
// check merged trajectory for collisions
|
||||||
|
if (!start_scene->isPathValid(*merged))
|
||||||
|
return;
|
||||||
|
|
||||||
SubTrajectory t(merged);
|
SubTrajectory t(merged);
|
||||||
// accumulate costs and markers
|
// accumulate costs and markers
|
||||||
double costs = 0.0;
|
double costs = 0.0;
|
||||||
|
|||||||
@ -227,7 +227,9 @@ SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTraject
|
|||||||
if (!trajectory)
|
if (!trajectory)
|
||||||
return SubTrajectoryPtr();
|
return SubTrajectoryPtr();
|
||||||
|
|
||||||
// TODO: check merged trajectory for collisions
|
// check merged trajectory for collisions
|
||||||
|
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);
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user