mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge pull request #151: Various fixes
This commit is contained in:
commit
f1acfa2f7e
@ -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
|
||||
*
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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.");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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));
|
||||
}
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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}
|
||||
|
||||
@ -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}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user