ComputeIK: Allow additional constraints for filtering solutions (#464)

Add "constraint" property.

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>

core/python/bindings/src/stages.cpp

@@ -191,10 +191,13 @@ void export_stages(pybind11::module& m) {
 			int: Set the maximum number of inverse
 			kinematic solutions thats should be generated.
 		)")
+	    .property<uint32_t>("max_ik_solutions", "uint: max number of solutions to return")
 	    .property<bool>("ignore_collisions", R"(
 			bool: Specify if collisions with other members of
 			the planning scene are allowed.
 		)")
+	    .property<double>("min_solution_distance", "reject solution that are closer than this to previously found solutions")
+	    .property<moveit_msgs::Constraints>("constraints", "additional constraints to obey")
 	    .property<geometry_msgs::PoseStamped>("ik_frame", R"(
 			PoseStamped_: Specify the frame with respect
 			to which the inverse kinematics

core/src/stages/compute_ik.cpp

@@ -63,6 +63,7 @@ ComputeIK::ComputeIK(const std::string& name, Stage::pointer&& child) : WrapperB
 	p.declare<bool>("ignore_collisions", false);
 	p.declare<double>("min_solution_distance", 0.1,
 	                  "minimum distance between seperate IK solutions for the same target");
+	p.declare<moveit_msgs::Constraints>("constraints", moveit_msgs::Constraints(), "additional constraints to obey");
 
 	// ik_frame and target_pose are read from the interface
 	p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
@@ -88,8 +89,9 @@ void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string&
 struct IKSolution
 {
 	std::vector<double> joint_positions;
-	bool feasible;
 	collision_detection::Contact contact;
+	bool collision_free;
+	bool satisfies_constraints;
 };
 
 using IKSolutions = std::vector<IKSolution>;
@@ -359,29 +361,40 @@ void ComputeIK::compute() {
 
 	double min_solution_distance = props.get<double>("min_solution_distance");
 
+	kinematic_constraints::KinematicConstraintSet constraint_set(robot_model);
+	constraint_set.add(props.get<moveit_msgs::Constraints>("constraints"), scene->getTransforms());
+
 	IKSolutions ik_solutions;
-	auto is_valid = [scene, ignore_collisions, min_solution_distance,
+	auto is_valid = [scene, ignore_collisions, min_solution_distance, &constraint_set = std::as_const(constraint_set),
 	                 &ik_solutions](moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg,
 	                                const double* joint_positions) {
 		for (const auto& sol : ik_solutions) {
 			if (jmg->distance(joint_positions, sol.joint_positions.data()) < min_solution_distance)
 				return false;  // too close to already found solution
 		}
 		state->setJointGroupPositions(jmg, joint_positions);
+		state->update();
+
 		ik_solutions.emplace_back();
 		auto& solution{ ik_solutions.back() };
 		state->copyJointGroupPositions(jmg, solution.joint_positions);
+
+		// validate constraints
+		solution.satisfies_constraints = constraint_set.decide(*state).satisfied;
+
+		// check for collisions
 		collision_detection::CollisionRequest req;
 		collision_detection::CollisionResult res;
 		req.contacts = true;
 		req.max_contacts = 1;
 		req.group_name = jmg->getName();
 		scene->checkCollision(req, res, *state);
-		solution.feasible = ignore_collisions || !res.collision;
+		solution.collision_free = ignore_collisions || !res.collision;
 		if (!res.contacts.empty()) {
 			solution.contact = res.contacts.begin()->second.front();
 		}
-		return solution.feasible;
+
+		return solution.satisfies_constraints && solution.collision_free;
 	};
 
 	uint32_t max_ik_solutions = props.get<uint32_t>("max_ik_solutions");
@@ -411,14 +424,16 @@ void ComputeIK::compute() {
 			solution.setComment(s.comment());
 			std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers()));
 
-			if (ik_solutions[i].feasible)
+			if (ik_solutions[i].collision_free && ik_solutions[i].satisfies_constraints)
 				// compute cost as distance to compare_pose
 				solution.setCost(s.cost() + jmg->distance(ik_solutions[i].joint_positions.data(), compare_pose.data()));
-			else {  // found an IK solution, but this was not valid
+			else if (!ik_solutions[i].collision_free) {  // solution was in collision
 				std::stringstream ss;
 				ss << "Collision between '" << ik_solutions[i].contact.body_name_1 << "' and '"
 				   << ik_solutions[i].contact.body_name_2 << "'";
 				solution.markAsFailure(ss.str());
+			} else if (!ik_solutions[i].satisfies_constraints) {  // solution was violating constraints
+				solution.markAsFailure("Constraints violated");
 			}
 			// set scene's robot state
 			moveit::core::RobotState& solution_state = solution_scene->getCurrentStateNonConst();