diff --git a/core/include/moveit/task_constructor/merge.h b/core/include/moveit/task_constructor/merge.h index 4ae50739..53d557e2 100644 --- a/core/include/moveit/task_constructor/merge.h +++ b/core/include/moveit/task_constructor/merge.h @@ -43,17 +43,10 @@ namespace moveit { namespace task_constructor { -/// create a new JointModelGroup comprising all joints of the given groups -moveit::core::JointModelGroup* merge(const std::vector& groups); - -/** find duplicate, non-fixed joints +/** Create a new JointModelGroup comprising all joints of the given groups * - * Merging is only allowed for disjoint joint sets. Fixed joints are tolerated. - * Assumes that \e joints is the the union of the joint sets of all \e groups (w/o duplicates). - * The list of duplicate joints is returned in \e duplicates and in \e names (as a comma-separated list) */ -bool findDuplicates(const std::vector& groups, - std::vector joints, - std::vector& duplicates, std::string& names); + * Throws if there are any duplicate, active joints in the groups */ +moveit::core::JointModelGroup* merge(const std::vector& groups); /** merge all sub trajectories into a single RobotTrajectory for parallel execution * diff --git a/core/src/container.cpp b/core/src/container.cpp index 7e0d5c33..1282f807 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -1112,8 +1112,13 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions, } moveit::core::JointModelGroup* jmg = jmg_merged_.get(); - robot_trajectory::RobotTrajectoryPtr merged = - task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg); + robot_trajectory::RobotTrajectoryPtr merged; + try { + merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg); + } catch (const std::runtime_error& e) { + ROS_INFO_STREAM_NAMED("Merger", this->name() << "Merging failed: " << e.what()); + return; + } if (jmg_merged_.get() != jmg) jmg_merged_.reset(jmg); if (!merged) diff --git a/core/src/merge.cpp b/core/src/merge.cpp index 56bacd10..db3b0c7c 100644 --- a/core/src/merge.cpp +++ b/core/src/merge.cpp @@ -38,6 +38,30 @@ #include #include +#include +#include + +namespace { +std::vector +findDuplicates(const std::vector& groups, + std::vector joints) { + std::vector duplicates; + for (const moveit::core::JointModelGroup* jmg : groups) { + for (const moveit::core::JointModel* jm : jmg->getJointModels()) { + auto it = std::find(joints.begin(), joints.end(), jm); + if (it == joints.end()) { // jm not found anymore -> duplicate + if (jm->getType() == moveit::core::JointModel::FIXED || jm->getMimic()) + continue; // fixed joints and mimic joints are OK + if (std::find(duplicates.begin(), duplicates.end(), jm) == duplicates.end()) + duplicates.push_back(jm); // add to duplicates only once + } else + joints.erase(it); // remove from list as processed + } + } + return duplicates; +} +} + namespace moveit { namespace task_constructor { @@ -48,7 +72,8 @@ moveit::core::JointModelGroup* merge(const std::vectorgetParentModel(); std::set jset; - std::vector names; + std::string merged_group_name; + size_t sum_joints = 0; for (const moveit::core::JointModelGroup* jmg : groups) { // sanity check: all groups must share the same robot model if (&jmg->getParentModel() != robot_model) @@ -56,36 +81,28 @@ moveit::core::JointModelGroup* merge(const std::vectorgetJointModels(); jset.insert(joints.cbegin(), joints.cend()); - names.push_back(jmg->getName()); + sum_joints += joints.size(); + if (!merged_group_name.empty()) + merged_group_name.append({ '+' }); + merged_group_name.append(jmg->getName()); } std::vector joints(jset.cbegin(), jset.cend()); - // attention: do not use getConfig() - return new moveit::core::JointModelGroup("", srdf::Model::Group(), joints, robot_model); -} - -bool findDuplicates(const std::vector& groups, - std::vector joints, - std::vector& duplicates, std::string& names) { - duplicates.clear(); - names.clear(); - for (const moveit::core::JointModelGroup* jmg : groups) { - for (const moveit::core::JointModel* jm : jmg->getJointModels()) { - auto it = std::find(joints.begin(), joints.end(), jm); - if (it == joints.end()) { // jm not found anymore -> duplicate - if (jm->getType() != moveit::core::JointModel::FIXED && // fixed joints are OK - std::find(duplicates.begin(), duplicates.end(), jm) == duplicates.end()) { - duplicates.push_back(jm); - if (!names.empty()) - names.append(", "); - names.append(jm->getName()); - } - continue; - } else - joints.erase(it); // remove from list as processed + if (joints.size() != sum_joints) { // overlapping joint groups: analyse in more detail + auto duplicates = findDuplicates(groups, joints); + if (!duplicates.empty()) { + std::string message( + "overlapping joints: " + + boost::algorithm::join(duplicates | boost::adaptors::transformed([](auto&& j) { return j->getName(); }), + ", ")); + throw std::runtime_error(message); } } - return duplicates.size() > 0; + + // JointModelGroup expects a srdf group, + // but this model is not constructed from an srdf, so we have to provide a dummy + static srdf::Model::Group dummy_srdf; + return new moveit::core::JointModelGroup(merged_group_name, dummy_srdf, joints, robot_model); } robot_trajectory::RobotTrajectoryPtr @@ -94,61 +111,36 @@ merge(const std::vector& sub_trajecto if (sub_trajectories.size() <= 1) throw std::runtime_error("Expected multiple sub solutions"); - const std::vector* merged_joints = - merged_group ? &merged_group->getJointModels() : nullptr; - std::set jset; - std::vector groups; - groups.reserve(sub_trajectories.size()); + if (!merged_group) { // create (and return) a merged group if not yet done + std::vector groups; + groups.reserve(sub_trajectories.size()); + for (const auto& sub : sub_trajectories) + groups.push_back(sub->getGroup()); + merged_group = merge(groups); + } + const std::vector* merged_joints = &merged_group->getJointModels(); // sanity checks: all sub solutions must share the same robot model and use disjoint joint sets 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 - + unsigned int max_num_vars = 0; // maximum number of joint variables across sub groups for (const robot_trajectory::RobotTrajectoryConstPtr& sub : sub_trajectories) { if (sub->getRobotModel() != robot_model) throw std::runtime_error("subsolutions refer to multiple robot models"); const moveit::core::JointModelGroup* jmg = sub->getGroup(); - groups.push_back(jmg); const auto& joints = jmg->getJointModels(); - if (merged_joints) { // validate that the joint model is known in merged_group - for (const moveit::core::JointModel* jm : joints) { - if (std::find(merged_joints->cbegin(), merged_joints->cend(), jm) == merged_joints->cend()) - throw std::runtime_error("subsolutions refers to unknown joint: " + jm->getName()); - } - } else // accumulate set of joints - jset.insert(joints.cbegin(), joints.cend()); - - max_num_joints = std::max(max_num_joints, joints.size()); - num_joints += joints.size(); - } - - size_t num_merged = merged_joints ? merged_joints->size() : jset.size(); - if (num_merged != num_joints) { - // overlapping joint groups: analyse in more detail - std::vector joints; - if (merged_joints) - joints = *merged_joints; - else - joints.insert(joints.end(), jset.cbegin(), jset.cend()); - - std::vector duplicates; - std::string names; - if (findDuplicates(groups, joints, duplicates, names)) - throw std::runtime_error("overlapping joint groups: " + names); - } - - // create merged_group if necessary - if (!merged_group) { - std::vector joints(jset.cbegin(), jset.cend()); - merged_group = new moveit::core::JointModelGroup("", srdf::Model::Group(), joints, robot_model.get()); + // validate that the joint model is known + for (const moveit::core::JointModel* jm : joints) { + if (std::find(merged_joints->cbegin(), merged_joints->cend(), jm) == merged_joints->cend()) + throw std::runtime_error("subsolutions refers to unknown joint: " + jm->getName()); + } + max_num_vars = std::max(max_num_vars, jmg->getVariableCount()); } // do the actual trajectory merging auto merged_traj = std::make_shared(robot_model, merged_group); std::vector values; - values.reserve(max_num_joints); + values.reserve(max_num_vars); auto merged_state = std::make_shared(base_state); while (true) { diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index c5f84934..211f91a7 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -83,16 +83,11 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) { } if (!errors && groups.size() >= 2) { // enable merging? - merged_jmg_.reset(task_constructor::merge(groups)); - if (merged_jmg_->getJointModels().size() != num_joints) { - // overlapping joint groups: analyse in more detail - std::vector duplicates; - std::string names; - if (findDuplicates(groups, merged_jmg_->getJointModels(), duplicates, names)) { - ROS_INFO_STREAM_NAMED("Connect", this->name() << ": overlapping joint groups: " << names - << ". Disabling merging."); - merged_jmg_.reset(); // fallback to serial connect - } + try { + merged_jmg_.reset(); + merged_jmg_.reset(task_constructor::merge(groups)); + } catch (const std::runtime_error& e) { + ROS_INFO_STREAM_NAMED("Connect", this->name() << ": " << e.what() << ". Disabling merging."); } }