From b22217deabf3ea66451cdb574805cd80791d5e29 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 10 Apr 2018 01:44:56 +0200 Subject: [PATCH] fix trajectory merging: initialize from well-defined RobotState --- core/include/moveit/task_constructor/merge.h | 2 +- .../include/moveit/task_constructor/stages/connect.h | 7 ++++++- core/src/merge.cpp | 12 +++++++----- core/src/stages/connect.cpp | 8 +++++--- 4 files changed, 19 insertions(+), 10 deletions(-) diff --git a/core/include/moveit/task_constructor/merge.h b/core/include/moveit/task_constructor/merge.h index 7e93261a..a75a8b7a 100644 --- a/core/include/moveit/task_constructor/merge.h +++ b/core/include/moveit/task_constructor/merge.h @@ -64,6 +64,6 @@ bool findDuplicates(const std::vector& gro * For now, only the trajectory path is considered. Timings, velocities, etc. are ignored. */ robot_trajectory::RobotTrajectoryPtr merge(const std::vector& sub_trajectories, - moveit::core::JointModelGroup*& merged_group); + const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group); } } diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index 521f4079..a37f057b 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -43,6 +43,10 @@ #include +namespace moveit { namespace core { +MOVEIT_CLASS_FORWARD(RobotState) +} } + namespace moveit { namespace task_constructor { namespace stages { class Connect : public Connecting { @@ -73,7 +77,8 @@ protected: SolutionBase* storeSequential(const std::vector& sub_trajectories, const std::vector& intermediate_scenes); robot_trajectory::RobotTrajectoryPtr merge(const std::vector& sub_trajectories, - const std::vector& intermediate_scenes); + const std::vector& intermediate_scenes, + const moveit::core::RobotState& state); protected: GroupPlannerVector planner_; diff --git a/core/src/merge.cpp b/core/src/merge.cpp index 39c58211..816f3e7d 100644 --- a/core/src/merge.cpp +++ b/core/src/merge.cpp @@ -89,7 +89,7 @@ bool findDuplicates(const std::vector& gro } robot_trajectory::RobotTrajectoryPtr merge(const std::vector& sub_trajectories, - moveit::core::JointModelGroup*& merged_group) + const robot_state::RobotState& base_state, moveit::core::JointModelGroup*& merged_group) { if (sub_trajectories.size() <= 1) throw std::runtime_error("Expected multiple sub solutions"); @@ -101,7 +101,7 @@ robot_trajectory::RobotTrajectoryPtr merge(const std::vectorgetRobotModel(); + const moveit::core::RobotModelConstPtr& robot_model = base_state.getRobotModel(); size_t max_num_joints = 0; // maximum number of joints in sub groups size_t num_joints = 0; // sum of joints in all sub groups @@ -148,11 +148,11 @@ robot_trajectory::RobotTrajectoryPtr merge(const std::vector values; values.reserve(max_num_joints); + auto merged_state = std::make_shared(base_state); while (true) { bool finished = true; size_t index = merged_traj->getWayPointCount(); - auto merged_state = index == 0 ? std::make_shared(robot_model) - : std::make_shared(merged_traj->getLastWayPoint()); + for (const robot_trajectory::RobotTrajectoryConstPtr& sub : sub_trajectories) { if (index >= sub->getWayPointCount()) continue; // no more waypoints in this sub solution @@ -161,13 +161,15 @@ robot_trajectory::RobotTrajectoryPtr merge(const std::vectorgetWayPoint(index); sub_state.copyJointGroupPositions(sub->getGroup(), values); merged_state->setJointGroupPositions(sub->getGroup(), values); + merged_state->update(); } if (finished) break; // add waypoint without timing - merged_state->update(); merged_traj->addSuffixWayPoint(merged_state, 0.0); + // create new RobotState for next waypoint + merged_state = std::make_shared(*merged_state); } return merged_traj; } diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 90ec3076..06d3332b 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -176,7 +176,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) { // mark solution as failure solution->setCost(std::numeric_limits::infinity()); } else { - robot_trajectory::RobotTrajectoryPtr t = merge(sub_trajectories, intermediate_scenes); + robot_trajectory::RobotTrajectoryPtr t = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState()); if (t) { connect(from, to, SubTrajectory(t)); return true; @@ -219,10 +219,12 @@ SolutionBase* Connect::storeSequential(const std::vector& sub_trajectories, - const std::vector& intermediate_scenes) + const std::vector& intermediate_scenes, + const moveit::core::RobotState& state) { auto jmg = merged_jmg_.get(); - robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, jmg); + robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg); + // TODO: check merged trajectory for collisions return trajectory; }