mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
simplify trajectory merging
avoid code duplication: reuse merge(std::vector<JointModelGroup*>)
This commit is contained in:
parent
718ad320c2
commit
8834ce18f7
@ -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
|
||||||
*
|
*
|
||||||
|
|||||||
@ -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)
|
||||||
|
|||||||
@ -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) {
|
||||||
|
|||||||
@ -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
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user