mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
report duplicate joints, accept fixed joints as duplicates
This commit is contained in:
parent
55e3aae30a
commit
047a3f7db4
@ -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<const moveit::core::JointModelGroup*>& 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<const moveit::core::JointModelGroup*>& groups,
|
||||
std::vector<const moveit::core::JointModel*> joints,
|
||||
std::vector<const moveit::core::JointModel*>& 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
|
||||
|
||||
@ -63,6 +63,31 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
|
||||
return new moveit::core::JointModelGroup("", srdf::Model::Group(), joints, robot_model);
|
||||
}
|
||||
|
||||
bool findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& groups,
|
||||
std::vector<const moveit::core::JointModel*> joints,
|
||||
std::vector<const moveit::core::JointModel*>& 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<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
|
||||
moveit::core::JointModelGroup*& merged_group)
|
||||
{
|
||||
@ -72,27 +97,44 @@ robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::R
|
||||
const std::vector<const moveit::core::JointModel*> *merged_joints
|
||||
= merged_group ? &merged_group->getJointModels() : nullptr;
|
||||
std::set<const moveit::core::JointModel*> jset;
|
||||
std::vector<const moveit::core::JointModelGroup*> 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<const moveit::core::JointModel*> joints;
|
||||
if (merged_joints) joints = *merged_joints;
|
||||
else joints.insert(joints.end(), jset.cbegin(), jset.cend());
|
||||
|
||||
std::vector<const moveit::core::JointModel*> duplicates;
|
||||
std::string names;
|
||||
if (findDuplicates(groups, joints, duplicates, names))
|
||||
throw std::runtime_error("overlapping joint groups: " + names);
|
||||
}
|
||||
|
||||
// create merged_group if necessary
|
||||
|
||||
@ -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<const moveit::core::JointModel*> 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)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user