mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
only attempt to merge if there is something to merge
This commit is contained in:
parent
51e1dda6ec
commit
433db67cf9
@ -76,7 +76,7 @@ public:
|
|||||||
protected:
|
protected:
|
||||||
SolutionBase* storeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
SolutionBase* storeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
||||||
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes);
|
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes);
|
||||||
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
robot_trajectory::RobotTrajectoryConstPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
||||||
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
|
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
|
||||||
const moveit::core::RobotState& state);
|
const moveit::core::RobotState& state);
|
||||||
|
|
||||||
|
|||||||
@ -176,7 +176,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
|
|||||||
// mark solution as failure
|
// mark solution as failure
|
||||||
solution->setCost(std::numeric_limits<double>::infinity());
|
solution->setCost(std::numeric_limits<double>::infinity());
|
||||||
} else {
|
} else {
|
||||||
robot_trajectory::RobotTrajectoryPtr t = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
|
auto t = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
|
||||||
if (t) {
|
if (t) {
|
||||||
connect(from, to, SubTrajectory(t));
|
connect(from, to, SubTrajectory(t));
|
||||||
return true;
|
return true;
|
||||||
@ -218,11 +218,16 @@ SolutionBase* Connect::storeSequential(const std::vector<robot_trajectory::Robot
|
|||||||
return &solutions_.back();
|
return &solutions_.back();
|
||||||
}
|
}
|
||||||
|
|
||||||
robot_trajectory::RobotTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
robot_trajectory::RobotTrajectoryConstPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
||||||
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
|
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
|
||||||
const moveit::core::RobotState& state)
|
const moveit::core::RobotState& state)
|
||||||
{
|
{
|
||||||
|
// no need to merge if there is only a single sub trajectory
|
||||||
|
if (sub_trajectories.size() == 1)
|
||||||
|
return sub_trajectories[0];
|
||||||
|
|
||||||
auto jmg = merged_jmg_.get();
|
auto jmg = merged_jmg_.get();
|
||||||
|
assert(jmg);
|
||||||
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg);
|
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg);
|
||||||
// TODO: check merged trajectory for collisions
|
// TODO: check merged trajectory for collisions
|
||||||
return trajectory;
|
return trajectory;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user