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
|
/// create a new JointModelGroup comprising all joints of the given groups
|
||||||
moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::JointModelGroup*>& 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
|
/** merge all sub trajectories into a single RobotTrajectory for parallel execution
|
||||||
*
|
*
|
||||||
* As the RobotTrajectory maintains a pointer to the underlying JointModelGroup
|
* 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);
|
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,
|
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
|
||||||
moveit::core::JointModelGroup*& merged_group)
|
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
|
const std::vector<const moveit::core::JointModel*> *merged_joints
|
||||||
= merged_group ? &merged_group->getJointModels() : nullptr;
|
= merged_group ? &merged_group->getJointModels() : nullptr;
|
||||||
std::set<const moveit::core::JointModel*> jset;
|
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
|
// 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();
|
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 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) {
|
for (const robot_trajectory::RobotTrajectoryPtr& sub : sub_trajectories) {
|
||||||
if (sub->getRobotModel() != robot_model)
|
if (sub->getRobotModel() != robot_model)
|
||||||
throw std::runtime_error("subsolutions refer to multiple robot models");
|
throw std::runtime_error("subsolutions refer to multiple robot models");
|
||||||
|
|
||||||
const moveit::core::JointModelGroup* jmg = sub->getGroup();
|
const moveit::core::JointModelGroup* jmg = sub->getGroup();
|
||||||
|
groups.push_back(jmg);
|
||||||
const auto& joints = jmg->getJointModels();
|
const auto& joints = jmg->getJointModels();
|
||||||
if (merged_joints) { // validate that the joint model is known in merged_group
|
if (merged_joints) { // validate that the joint model is known in merged_group
|
||||||
for (const moveit::core::JointModel* jm : joints) {
|
for (const moveit::core::JointModel* jm : joints) {
|
||||||
if (std::find(merged_joints->cbegin(), merged_joints->cend(), jm) == merged_joints->cend())
|
if (std::find(merged_joints->cbegin(), merged_joints->cend(), jm) == merged_joints->cend())
|
||||||
throw std::runtime_error("subsolutions refers to unknown joint: " + jm->getName());
|
throw std::runtime_error("subsolutions refers to unknown joint: " + jm->getName());
|
||||||
}
|
}
|
||||||
}
|
} else // accumulate set of joints
|
||||||
max_num_joints = std::max(max_num_joints, joints.size());
|
|
||||||
size_t expected_size = jset.size() + joints.size();
|
|
||||||
jset.insert(joints.cbegin(), joints.cend());
|
jset.insert(joints.cbegin(), joints.cend());
|
||||||
if (jset.size() != expected_size)
|
|
||||||
throw std::runtime_error("joint sets of subsolutions are not disjoint");
|
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<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
|
// create merged_group if necessary
|
||||||
|
|||||||
@ -85,10 +85,18 @@ void Connect::init(const core::RobotModelConstPtr& robot_model)
|
|||||||
num_joints += jmg->getJointModels().size();
|
num_joints += jmg->getJointModels().size();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (!errors && groups.size() >= 2) {
|
|
||||||
|
if (!errors && groups.size() >= 2) { // enable merging?
|
||||||
merged_jmg_.reset(task_constructor::merge(groups));
|
merged_jmg_.reset(task_constructor::merge(groups));
|
||||||
if (merged_jmg_->getJointModels().size() != num_joints)
|
if (merged_jmg_->getJointModels().size() != num_joints) {
|
||||||
errors.push_back(*this, "overlapping joint groups");
|
// 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)
|
if (errors)
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user