diff --git a/core/include/moveit/task_constructor/merge.h b/core/include/moveit/task_constructor/merge.h index c8746567..1d6a97d1 100644 --- a/core/include/moveit/task_constructor/merge.h +++ b/core/include/moveit/task_constructor/merge.h @@ -46,6 +46,16 @@ 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 + * + * 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); + /** merge all sub trajectories into a single RobotTrajectory for parallel execution * * As the RobotTrajectory maintains a pointer to the underlying JointModelGroup diff --git a/core/src/merge.cpp b/core/src/merge.cpp index 7a24ec72..25ea3dee 100644 --- a/core/src/merge.cpp +++ b/core/src/merge.cpp @@ -63,6 +63,31 @@ moveit::core::JointModelGroup* merge(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 + } + } + return duplicates.size() > 0; +} + robot_trajectory::RobotTrajectoryPtr merge(const std::vector& sub_trajectories, moveit::core::JointModelGroup*& merged_group) { @@ -72,27 +97,44 @@ robot_trajectory::RobotTrajectoryPtr merge(const std::vector *merged_joints = merged_group ? &merged_group->getJointModels() : nullptr; std::set jset; + std::vector groups; + groups.reserve(sub_trajectories.size()); // sanity checks: all sub solutions must share the same robot model and use disjoint joint sets const moveit::core::RobotModelConstPtr& robot_model = sub_trajectories[0]->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 + for (const robot_trajectory::RobotTrajectoryPtr& 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()); - size_t expected_size = jset.size() + joints.size(); - jset.insert(joints.cbegin(), joints.cend()); - if (jset.size() != expected_size) - throw std::runtime_error("joint sets of subsolutions are not disjoint"); + 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 diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index c25cdf81..eaa78e94 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -85,10 +85,18 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) num_joints += jmg->getJointModels().size(); } } - if (!errors && groups.size() >= 2) { + + if (!errors && groups.size() >= 2) { // enable merging? merged_jmg_.reset(task_constructor::merge(groups)); - if (merged_jmg_->getJointModels().size() != num_joints) - errors.push_back(*this, "overlapping joint 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 + } + } } if (errors)