Merge pull request #151: Various fixes

Author: rhaschke

core/include/moveit/task_constructor/merge.h

@@ -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
  *

core/src/container.cpp

@@ -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)

core/src/merge.cpp

@@ -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,44 +72,37 @@ 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)
 			throw std::runtime_error("groups refer to different robot models");
 
 		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

core/src/solvers/joint_interpolation.cpp

@@ -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;

core/src/stages/connect.cpp

@@ -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.");
 		}
 	}
 

demo/src/cartesian.cpp

@@ -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));
 	}

visualization/CMakeLists.txt

@@ -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)

visualization/motion_planning_tasks/src/CMakeLists.txt

@@ -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}

visualization/motion_planning_tasks/utils/CMakeLists.txt

@@ -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}