simplify trajectory merging

avoid code duplication: reuse merge(std::vector<JointModelGroup*>)
This commit is contained in:
Robert Haschke 2020-03-04 11:38:12 +01:00
parent 718ad320c2
commit 8834ce18f7
4 changed files with 74 additions and 89 deletions

View File

@ -43,17 +43,10 @@
namespace moveit { namespace moveit {
namespace task_constructor { 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);
/** find duplicate, non-fixed joints
* *
* Merging is only allowed for disjoint joint sets. Fixed joints are tolerated. * Throws if there are any duplicate, active joints in the groups */
* Assumes that \e joints is the the union of the joint sets of all \e groups (w/o duplicates). moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::JointModelGroup*>& groups);
* 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
* *

View File

@ -1112,8 +1112,13 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions,
} }
moveit::core::JointModelGroup* jmg = jmg_merged_.get(); moveit::core::JointModelGroup* jmg = jmg_merged_.get();
robot_trajectory::RobotTrajectoryPtr merged = robot_trajectory::RobotTrajectoryPtr merged;
task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg); 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) if (jmg_merged_.get() != jmg)
jmg_merged_.reset(jmg); jmg_merged_.reset(jmg);
if (!merged) if (!merged)

View File

@ -38,6 +38,30 @@
#include <moveit/task_constructor/merge.h> #include <moveit/task_constructor/merge.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h> #include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/algorithm/string/join.hpp>
namespace {
std::vector<const moveit::core::JointModel*>
findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& groups,
std::vector<const moveit::core::JointModel*> joints) {
std::vector<const moveit::core::JointModel*> 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 moveit {
namespace task_constructor { namespace task_constructor {
@ -48,7 +72,8 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
const moveit::core::RobotModel* const robot_model = &groups[0]->getParentModel(); const moveit::core::RobotModel* const robot_model = &groups[0]->getParentModel();
std::set<const moveit::core::JointModel*> jset; std::set<const moveit::core::JointModel*> jset;
std::vector<std::string> names; std::string merged_group_name;
size_t sum_joints = 0;
for (const moveit::core::JointModelGroup* jmg : groups) { for (const moveit::core::JointModelGroup* jmg : groups) {
// sanity check: all groups must share the same robot model // sanity check: all groups must share the same robot model
if (&jmg->getParentModel() != robot_model) if (&jmg->getParentModel() != robot_model)
@ -56,36 +81,28 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
const auto& joints = jmg->getJointModels(); const auto& joints = jmg->getJointModels();
jset.insert(joints.cbegin(), joints.cend()); 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<const moveit::core::JointModel*> joints(jset.cbegin(), jset.cend()); std::vector<const moveit::core::JointModel*> joints(jset.cbegin(), jset.cend());
// attention: do not use getConfig() if (joints.size() != sum_joints) { // overlapping joint groups: analyse in more detail
return new moveit::core::JointModelGroup("", srdf::Model::Group(), joints, robot_model); auto duplicates = findDuplicates(groups, joints);
} if (!duplicates.empty()) {
std::string message(
bool findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& groups, "overlapping joints: " +
std::vector<const moveit::core::JointModel*> joints, boost::algorithm::join(duplicates | boost::adaptors::transformed([](auto&& j) { return j->getName(); }),
std::vector<const moveit::core::JointModel*>& duplicates, std::string& names) { ", "));
duplicates.clear(); throw std::runtime_error(message);
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;
// 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 robot_trajectory::RobotTrajectoryPtr
@ -94,61 +111,36 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
if (sub_trajectories.size() <= 1) if (sub_trajectories.size() <= 1)
throw std::runtime_error("Expected multiple sub solutions"); throw std::runtime_error("Expected multiple sub solutions");
const std::vector<const moveit::core::JointModel*>* merged_joints = if (!merged_group) { // create (and return) a merged group if not yet done
merged_group ? &merged_group->getJointModels() : nullptr; std::vector<const moveit::core::JointModelGroup*> groups;
std::set<const moveit::core::JointModel*> jset; groups.reserve(sub_trajectories.size());
std::vector<const moveit::core::JointModelGroup*> groups; for (const auto& sub : sub_trajectories)
groups.reserve(sub_trajectories.size()); groups.push_back(sub->getGroup());
merged_group = merge(groups);
}
const std::vector<const moveit::core::JointModel*>* merged_joints = &merged_group->getJointModels();
// 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 = base_state.getRobotModel(); const moveit::core::RobotModelConstPtr& robot_model = base_state.getRobotModel();
size_t max_num_joints = 0; // maximum number of joints in sub groups unsigned int max_num_vars = 0; // maximum number of joint variables across sub groups
size_t num_joints = 0; // sum of joints in all sub groups
for (const robot_trajectory::RobotTrajectoryConstPtr& sub : sub_trajectories) { for (const robot_trajectory::RobotTrajectoryConstPtr& 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 // validate that the joint model is known
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_vars = std::max(max_num_vars, jmg->getVariableCount());
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<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
if (!merged_group) {
std::vector<const moveit::core::JointModel*> joints(jset.cbegin(), jset.cend());
merged_group = new moveit::core::JointModelGroup("", srdf::Model::Group(), joints, robot_model.get());
} }
// do the actual trajectory merging // do the actual trajectory merging
auto merged_traj = std::make_shared<robot_trajectory::RobotTrajectory>(robot_model, merged_group); auto merged_traj = std::make_shared<robot_trajectory::RobotTrajectory>(robot_model, merged_group);
std::vector<double> values; std::vector<double> values;
values.reserve(max_num_joints); values.reserve(max_num_vars);
auto merged_state = std::make_shared<robot_state::RobotState>(base_state); auto merged_state = std::make_shared<robot_state::RobotState>(base_state);
while (true) { while (true) {

View File

@ -83,16 +83,11 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) {
} }
if (!errors && groups.size() >= 2) { // enable merging? if (!errors && groups.size() >= 2) { // enable merging?
merged_jmg_.reset(task_constructor::merge(groups)); try {
if (merged_jmg_->getJointModels().size() != num_joints) { merged_jmg_.reset();
// overlapping joint groups: analyse in more detail merged_jmg_.reset(task_constructor::merge(groups));
std::vector<const moveit::core::JointModel*> duplicates; } catch (const std::runtime_error& e) {
std::string names; ROS_INFO_STREAM_NAMED("Connect", this->name() << ": " << e.what() << ". Disabling merging.");
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
}
} }
} }