Skip to content

Commit 8834ce1

Browse files
rhaschkerhaschke
committed
simplify trajectory merging
avoid code duplication: reuse merge(std::vector<JointModelGroup*>)
1 parent 718ad32 commit 8834ce1

File tree

4 files changed

+74
-89
lines changed

4 files changed

+74
-89
lines changed

‎core/include/moveit/task_constructor/merge.h‎

Lines changed: 3 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -43,17 +43,10 @@
4343
namespace moveit {
4444
namespace task_constructor {
4545

46-
/// create a new JointModelGroup comprising all joints of the given groups
47-
moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::JointModelGroup*>& groups);
48-
49-
/** find duplicate, non-fixed joints
46+
/** Create a new JointModelGroup comprising all joints of the given groups
5047
*
51-
* Merging is only allowed for disjoint joint sets. Fixed joints are tolerated.
52-
* Assumes that \e joints is the the union of the joint sets of all \e groups (w/o duplicates).
53-
* The list of duplicate joints is returned in \e duplicates and in \e names (as a comma-separated list) */
54-
bool findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& groups,
55-
std::vector<const moveit::core::JointModel*> joints,
56-
std::vector<const moveit::core::JointModel*>& duplicates, std::string& names);
48+
* Throws if there are any duplicate, active joints in the groups */
49+
moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::JointModelGroup*>& groups);
5750

5851
/** merge all sub trajectories into a single RobotTrajectory for parallel execution
5952
*

‎core/src/container.cpp‎

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1112,8 +1112,13 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions,
11121112
}
11131113

11141114
moveit::core::JointModelGroup* jmg = jmg_merged_.get();
1115-
robot_trajectory::RobotTrajectoryPtr merged =
1116-
task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg);
1115+
robot_trajectory::RobotTrajectoryPtr merged;
1116+
try {
1117+
merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg);
1118+
} catch (const std::runtime_error& e) {
1119+
ROS_INFO_STREAM_NAMED("Merger", this->name() << "Merging failed: " << e.what());
1120+
return;
1121+
}
11171122
if (jmg_merged_.get() != jmg)
11181123
jmg_merged_.reset(jmg);
11191124
if (!merged)

‎core/src/merge.cpp‎

Lines changed: 59 additions & 67 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,30 @@
3838
#include <moveit/task_constructor/merge.h>
3939
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
4040

41+
#include <boost/range/adaptor/transformed.hpp>
42+
#include <boost/algorithm/string/join.hpp>
43+
44+
namespace {
45+
std::vector<const moveit::core::JointModel*>
46+
findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& groups,
47+
std::vector<const moveit::core::JointModel*> joints) {
48+
std::vector<const moveit::core::JointModel*> duplicates;
49+
for (const moveit::core::JointModelGroup* jmg : groups) {
50+
for (const moveit::core::JointModel* jm : jmg->getJointModels()) {
51+
auto it = std::find(joints.begin(), joints.end(), jm);
52+
if (it == joints.end()) { // jm not found anymore -> duplicate
53+
if (jm->getType() == moveit::core::JointModel::FIXED || jm->getMimic())
54+
continue; // fixed joints and mimic joints are OK
55+
if (std::find(duplicates.begin(), duplicates.end(), jm) == duplicates.end())
56+
duplicates.push_back(jm); // add to duplicates only once
57+
} else
58+
joints.erase(it); // remove from list as processed
59+
}
60+
}
61+
return duplicates;
62+
}
63+
}
64+
4165
namespace moveit {
4266
namespace task_constructor {
4367

@@ -48,44 +72,37 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
4872
const moveit::core::RobotModel* const robot_model = &groups[0]->getParentModel();
4973

5074
std::set<const moveit::core::JointModel*> jset;
51-
std::vector<std::string> names;
75+
std::string merged_group_name;
76+
size_t sum_joints = 0;
5277
for (const moveit::core::JointModelGroup* jmg : groups) {
5378
// sanity check: all groups must share the same robot model
5479
if (&jmg->getParentModel() != robot_model)
5580
throw std::runtime_error("groups refer to different robot models");
5681

5782
const auto& joints = jmg->getJointModels();
5883
jset.insert(joints.cbegin(), joints.cend());
59-
names.push_back(jmg->getName());
84+
sum_joints += joints.size();
85+
if (!merged_group_name.empty())
86+
merged_group_name.append({ '+' });
87+
merged_group_name.append(jmg->getName());
6088
}
6189

6290
std::vector<const moveit::core::JointModel*> joints(jset.cbegin(), jset.cend());
63-
// attention: do not use getConfig()
64-
return new moveit::core::JointModelGroup("", srdf::Model::Group(), joints, robot_model);
65-
}
66-
67-
bool findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& groups,
68-
std::vector<const moveit::core::JointModel*> joints,
69-
std::vector<const moveit::core::JointModel*>& duplicates, std::string& names) {
70-
duplicates.clear();
71-
names.clear();
72-
for (const moveit::core::JointModelGroup* jmg : groups) {
73-
for (const moveit::core::JointModel* jm : jmg->getJointModels()) {
74-
auto it = std::find(joints.begin(), joints.end(), jm);
75-
if (it == joints.end()) { // jm not found anymore -> duplicate
76-
if (jm->getType() != moveit::core::JointModel::FIXED && // fixed joints are OK
77-
std::find(duplicates.begin(), duplicates.end(), jm) == duplicates.end()) {
78-
duplicates.push_back(jm);
79-
if (!names.empty())
80-
names.append(", ");
81-
names.append(jm->getName());
82-
}
83-
continue;
84-
} else
85-
joints.erase(it); // remove from list as processed
91+
if (joints.size() != sum_joints) { // overlapping joint groups: analyse in more detail
92+
auto duplicates = findDuplicates(groups, joints);
93+
if (!duplicates.empty()) {
94+
std::string message(
95+
"overlapping joints: " +
96+
boost::algorithm::join(duplicates | boost::adaptors::transformed([](auto&& j) { return j->getName(); }),
97+
", "));
98+
throw std::runtime_error(message);
8699
}
87100
}
88-
return duplicates.size() > 0;
101+
102+
// JointModelGroup expects a srdf group,
103+
// but this model is not constructed from an srdf, so we have to provide a dummy
104+
static srdf::Model::Group dummy_srdf;
105+
return new moveit::core::JointModelGroup(merged_group_name, dummy_srdf, joints, robot_model);
89106
}
90107

91108
robot_trajectory::RobotTrajectoryPtr
@@ -94,61 +111,36 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
94111
if (sub_trajectories.size() <= 1)
95112
throw std::runtime_error("Expected multiple sub solutions");
96113

97-
const std::vector<const moveit::core::JointModel*>* merged_joints =
98-
merged_group ? &merged_group->getJointModels() : nullptr;
99-
std::set<const moveit::core::JointModel*> jset;
100-
std::vector<const moveit::core::JointModelGroup*> groups;
101-
groups.reserve(sub_trajectories.size());
114+
if (!merged_group) { // create (and return) a merged group if not yet done
115+
std::vector<const moveit::core::JointModelGroup*> groups;
116+
groups.reserve(sub_trajectories.size());
117+
for (const auto& sub : sub_trajectories)
118+
groups.push_back(sub->getGroup());
119+
merged_group = merge(groups);
120+
}
121+
const std::vector<const moveit::core::JointModel*>* merged_joints = &merged_group->getJointModels();
102122

103123
// sanity checks: all sub solutions must share the same robot model and use disjoint joint sets
104124
const moveit::core::RobotModelConstPtr& robot_model = base_state.getRobotModel();
105-
size_t max_num_joints = 0; // maximum number of joints in sub groups
106-
size_t num_joints = 0; // sum of joints in all sub groups
107-
125+
unsigned int max_num_vars = 0; // maximum number of joint variables across sub groups
108126
for (const robot_trajectory::RobotTrajectoryConstPtr& sub : sub_trajectories) {
109127
if (sub->getRobotModel() != robot_model)
110128
throw std::runtime_error("subsolutions refer to multiple robot models");
111129

112130
const moveit::core::JointModelGroup* jmg = sub->getGroup();
113-
groups.push_back(jmg);
114131
const auto& joints = jmg->getJointModels();
115-
if (merged_joints) { // validate that the joint model is known in merged_group
116-
for (const moveit::core::JointModel* jm : joints) {
117-
if (std::find(merged_joints->cbegin(), merged_joints->cend(), jm) == merged_joints->cend())
118-
throw std::runtime_error("subsolutions refers to unknown joint: " + jm->getName());
119-
}
120-
} else // accumulate set of joints
121-
jset.insert(joints.cbegin(), joints.cend());
122-
123-
max_num_joints = std::max(max_num_joints, joints.size());
124-
num_joints += joints.size();
125-
}
126-
127-
size_t num_merged = merged_joints ? merged_joints->size() : jset.size();
128-
if (num_merged != num_joints) {
129-
// overlapping joint groups: analyse in more detail
130-
std::vector<const moveit::core::JointModel*> joints;
131-
if (merged_joints)
132-
joints = *merged_joints;
133-
else
134-
joints.insert(joints.end(), jset.cbegin(), jset.cend());
135-
136-
std::vector<const moveit::core::JointModel*> duplicates;
137-
std::string names;
138-
if (findDuplicates(groups, joints, duplicates, names))
139-
throw std::runtime_error("overlapping joint groups: " + names);
140-
}
141-
142-
// create merged_group if necessary
143-
if (!merged_group) {
144-
std::vector<const moveit::core::JointModel*> joints(jset.cbegin(), jset.cend());
145-
merged_group = new moveit::core::JointModelGroup("", srdf::Model::Group(), joints, robot_model.get());
132+
// validate that the joint model is known
133+
for (const moveit::core::JointModel* jm : joints) {
134+
if (std::find(merged_joints->cbegin(), merged_joints->cend(), jm) == merged_joints->cend())
135+
throw std::runtime_error("subsolutions refers to unknown joint: " + jm->getName());
136+
}
137+
max_num_vars = std::max(max_num_vars, jmg->getVariableCount());
146138
}
147139

148140
// do the actual trajectory merging
149141
auto merged_traj = std::make_shared<robot_trajectory::RobotTrajectory>(robot_model, merged_group);
150142
std::vector<double> values;
151-
values.reserve(max_num_joints);
143+
values.reserve(max_num_vars);
152144

153145
auto merged_state = std::make_shared<robot_state::RobotState>(base_state);
154146
while (true) {

‎core/src/stages/connect.cpp‎

Lines changed: 5 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -83,16 +83,11 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) {
8383
}
8484

8585
if (!errors && groups.size() >= 2) { // enable merging?
86-
merged_jmg_.reset(task_constructor::merge(groups));
87-
if (merged_jmg_->getJointModels().size() != num_joints) {
88-
// overlapping joint groups: analyse in more detail
89-
std::vector<const moveit::core::JointModel*> duplicates;
90-
std::string names;
91-
if (findDuplicates(groups, merged_jmg_->getJointModels(), duplicates, names)) {
92-
ROS_INFO_STREAM_NAMED("Connect", this->name() << ": overlapping joint groups: " << names
93-
<< ". Disabling merging.");
94-
merged_jmg_.reset(); // fallback to serial connect
95-
}
86+
try {
87+
merged_jmg_.reset();
88+
merged_jmg_.reset(task_constructor::merge(groups));
89+
} catch (const std::runtime_error& e) {
90+
ROS_INFO_STREAM_NAMED("Connect", this->name() << ": " << e.what() << ". Disabling merging.");
9691
}
9792
}
9893

0 commit comments

Comments
 (0)