Merge pull request #151: Various fixes

This commit is contained in:
Robert Haschke 2020-04-03 11:58:19 +02:00 committed by GitHub
commit f1acfa2f7e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 105 additions and 105 deletions

View File

@ -43,17 +43,10 @@
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
/** Create a new JointModelGroup comprising all joints of the given groups
*
* 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);
* Throws if there are any duplicate, active joints in the groups */
moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::JointModelGroup*>& groups);
/** merge all sub trajectories into a single RobotTrajectory for parallel execution
*

View File

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

View File

@ -38,6 +38,30 @@
#include <moveit/task_constructor/merge.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 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();
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) {
// sanity check: all groups must share the same 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();
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());
// attention: do not use getConfig()
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
if (joints.size() != sum_joints) { // overlapping joint groups: analyse in more detail
auto duplicates = findDuplicates(groups, joints);
if (!duplicates.empty()) {
std::string message(
"overlapping joints: " +
boost::algorithm::join(duplicates | boost::adaptors::transformed([](auto&& j) { return j->getName(); }),
", "));
throw std::runtime_error(message);
}
}
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
@ -94,61 +111,36 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
if (sub_trajectories.size() <= 1)
throw std::runtime_error("Expected multiple sub solutions");
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());
if (!merged_group) { // create (and return) a merged group if not yet done
std::vector<const moveit::core::JointModelGroup*> groups;
groups.reserve(sub_trajectories.size());
for (const auto& sub : sub_trajectories)
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
const moveit::core::RobotModelConstPtr& robot_model = base_state.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
unsigned int max_num_vars = 0; // maximum number of joint variables across sub groups
for (const robot_trajectory::RobotTrajectoryConstPtr& 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());
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());
// validate that the joint model is known
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());
}
max_num_vars = std::max(max_num_vars, jmg->getVariableCount());
}
// do the actual trajectory merging
auto merged_traj = std::make_shared<robot_trajectory::RobotTrajectory>(robot_model, merged_group);
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);
while (true) {
@ -163,11 +155,11 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
const robot_state::RobotState& sub_state = sub->getWayPoint(index);
sub_state.copyJointGroupPositions(sub->getGroup(), values);
merged_state->setJointGroupPositions(sub->getGroup(), values);
merged_state->update();
}
if (finished)
break;
merged_state->update();
// add waypoint without timing
merged_traj->addSuffixWayPoint(merged_state, 0.0);
// create new RobotState for next waypoint

View File

@ -64,16 +64,16 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
const moveit::core::RobotState& to_state = to->getCurrentState();
for (const moveit::core::JointModel* jm : from_state.getRobotModel()->getActiveJointModels())
d = std::max(d, jm->getDistanceFactor() * from_state.distance(to_state, jm));
if (d < 1e-6)
return true; // return empty result trajectory: no motion needed
result = std::make_shared<robot_trajectory::RobotTrajectory>(from->getRobotModel(), jmg);
// add first point
result->addSuffixWayPoint(from->getCurrentState(), 0.0);
if (from->isStateColliding(from_state, jmg->getName()))
return false;
moveit::core::RobotState waypoint(from_state);
double delta = props.get<double>("max_step") / d;
double delta = d < 1e-6 ? 1.0 : props.get<double>("max_step") / d;
for (double t = delta; t < 1.0; t += delta) {
from_state.interpolate(to_state, t, waypoint);
result->addSuffixWayPoint(waypoint, t);
@ -83,7 +83,9 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
}
// add goal point
result->addSuffixWayPoint(to->getCurrentState(), 1.0);
result->addSuffixWayPoint(to_state, 1.0);
if (from->isStateColliding(to_state, jmg->getName()))
return false;
// add timing, TODO: use a generic method to add timing via plugins
trajectory_processing::IterativeParabolicTimeParameterization timing;

View File

@ -83,16 +83,11 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) {
}
if (!errors && groups.size() >= 2) { // enable merging?
merged_jmg_.reset(task_constructor::merge(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
}
try {
merged_jmg_.reset();
merged_jmg_.reset(task_constructor::merge(groups));
} catch (const std::runtime_error& e) {
ROS_INFO_STREAM_NAMED("Connect", this->name() << ": " << e.what() << ". Disabling merging.");
}
}

View File

@ -51,9 +51,12 @@ Task createTask() {
t.stages()->setName("Cartesian Path");
const std::string group = "panda_arm";
const std::string eef = "hand";
// create Cartesian interpolation "planner" to be used in stages
auto cartesian = std::make_shared<solvers::CartesianPath>();
// create Cartesian interpolation "planner" to be used in various stages
auto cartesian_interpolation = std::make_shared<solvers::CartesianPath>();
// create a joint-space interpolation "planner" to be used in various stages
auto joint_interpolation = std::make_shared<solvers::JointInterpolationPlanner>();
// start from a fixed robot state
t.loadRobotModel();
@ -68,7 +71,7 @@ Task createTask() {
}
{
auto stage = std::make_unique<stages::MoveRelative>("x +0.2", cartesian);
auto stage = std::make_unique<stages::MoveRelative>("x +0.2", cartesian_interpolation);
stage->setGroup(group);
geometry_msgs::Vector3Stamped direction;
direction.header.frame_id = "world";
@ -78,7 +81,7 @@ Task createTask() {
}
{
auto stage = std::make_unique<stages::MoveRelative>("y -0.3", cartesian);
auto stage = std::make_unique<stages::MoveRelative>("y -0.3", cartesian_interpolation);
stage->setGroup(group);
geometry_msgs::Vector3Stamped direction;
direction.header.frame_id = "world";
@ -88,7 +91,7 @@ Task createTask() {
}
{ // rotate about TCP
auto stage = std::make_unique<stages::MoveRelative>("rz +45°", cartesian);
auto stage = std::make_unique<stages::MoveRelative>("rz +45°", cartesian_interpolation);
stage->setGroup(group);
geometry_msgs::TwistStamped twist;
twist.header.frame_id = "world";
@ -98,16 +101,23 @@ Task createTask() {
}
{ // perform a Cartesian motion, defined as a relative offset in joint space
auto stage = std::make_unique<stages::MoveRelative>("joint offset", cartesian);
auto stage = std::make_unique<stages::MoveRelative>("joint offset", cartesian_interpolation);
stage->setGroup(group);
std::map<std::string, double> offsets = { { "panda_joint1", M_PI / 6. }, { "panda_joint3", -M_PI / 6 } };
stage->setDirection(offsets);
t.add(std::move(stage));
}
{ // move gripper into predefined open state
auto stage = std::make_unique<stages::MoveTo>("open gripper", joint_interpolation);
stage->setGroup(eef);
stage->setGoal("open");
t.add(std::move(stage));
}
{ // move from reached state back to the original state, using joint interpolation
auto joint_interpolation = std::make_shared<solvers::JointInterpolationPlanner>();
stages::Connect::GroupPlannerVector planners = { { group, joint_interpolation } };
// specifying two groups (arm and hand) will try to merge both trajectories
stages::Connect::GroupPlannerVector planners = { { group, joint_interpolation }, { eef, joint_interpolation } };
auto connect = std::make_unique<stages::Connect>("connect", planners);
t.add(std::move(connect));
}

View File

@ -28,7 +28,6 @@ else()
endmacro()
endif()
set(CMAKE_INCLUDE_CURRENT_DIR ON)
set(CMAKE_AUTOMOC ON)
set(CMAKE_AUTORCC ON)
add_definitions(-DQT_NO_KEYWORDS)

View File

@ -33,7 +33,11 @@ target_link_libraries(${MOVEIT_LIB_NAME}
target_include_directories(${MOVEIT_LIB_NAME}
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
PRIVATE ${catkin_INCLUDE_DIRS}
# https://stackoverflow.com/questions/47175683/cmake-target-link-libraries-propagation-of-include-directories
PUBLIC $<TARGET_PROPERTY:motion_planning_tasks_utils,INTERFACE_INCLUDE_DIRECTORIES>
PUBLIC $<TARGET_PROPERTY:motion_planning_tasks_properties,INTERFACE_INCLUDE_DIRECTORIES>
PUBLIC $<TARGET_PROPERTY:moveit_task_visualization_tools,INTERFACE_INCLUDE_DIRECTORIES>
PUBLIC ${catkin_INCLUDE_DIRS}
)
install(TARGETS ${MOVEIT_LIB_NAME}

View File

@ -12,7 +12,7 @@ target_link_libraries(${MOVEIT_LIB_NAME}
)
target_include_directories(${MOVEIT_LIB_NAME}
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/..>
PUBLIC ${catkin_INCLUDE_DIRS}
PRIVATE ${catkin_INCLUDE_DIRS}
)
install(TARGETS ${MOVEIT_LIB_NAME}