LimitSolutions wrapper stage (#710)

* Add python bindings for PassThrough
* Add LimitSolutions stage

Author: rhaschke

core/include/moveit/task_constructor/stages.h

@@ -48,7 +48,9 @@
 #include "stages/generate_place_pose.h"
 #include "stages/generate_pose.h"
 #include "stages/generate_random_pose.h"
+#include "stages/limit_solutions.h"
 #include "stages/modify_planning_scene.h"
 #include "stages/move_relative.h"
 #include "stages/move_to.h"
+#include "stages/passthrough.h"
 #include "stages/predicate_filter.h"

core/include/moveit/task_constructor/stages/limit_solutions.h

@@ -0,0 +1,65 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holders nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+/* Authors: Joseph Moore */
+
+#pragma once
+
+#include <moveit/task_constructor/container.h>
+
+namespace moveit {
+namespace task_constructor {
+namespace stages {
+
+/** A wrapper which lazily passes a limited number of solutions
+ *
+ * The best solution at each call to compute is passed on
+ *
+ */
+class LimitSolutions : public WrapperBase
+{
+public:
+	LimitSolutions(const std::string& name = "LimitSolutions", Stage::pointer&& child = Stage::pointer());
+
+	void reset() override;
+	bool canCompute() const override;
+	void compute() override;
+	void onNewSolution(const SolutionBase& s) override;
+
+	void setMaxSolutions(uint32_t max_solutions) { setProperty("max_solutions", max_solutions); }
+
+private:
+	ordered<const SolutionBase*> upstream_solutions_;
+	uint32_t forwarded_solutions;
+};
+}  // namespace stages
+}  // namespace task_constructor
+}  // namespace moveit

core/python/bindings/src/stages.cpp

@@ -171,10 +171,6 @@ void export_stages(pybind11::module& m) {
 			str: Default joint pose of the active group
 			(defines cost of the inverse kinematics).
 		)")
-	    .property<uint32_t>("max_ik_solutions", R"(
-			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
@@ -548,6 +544,23 @@ void export_stages(pybind11::module& m) {
 		.property<std::string>("grasp", "str: Name of the grasp pose")
 		.def(py::init<Stage::pointer&&, const std::string&>(), "pose_generator"_a,
 		     "name"_a = std::string("ungrasp"));
+
+	properties::class_<PassThrough, Stage>(m, "PassThrough", R"(
+			Wrapper which simply forwards all solutions of a child stage,
+			allowing the wrapper to redefine costs, w/o loosing original costs.
+		)")
+	    .def(py::init<const std::string&, Stage::pointer&&>(), "name"_a, "stage"_a);
+
+	properties::class_<LimitSolutions, Stage>(m, "LimitSolutions", R"(
+			Wrapper for any stage to limit the total number of solutions returned.
+
+			The wrapper stores solutions of its child stage, and on each compute will
+			pass on the lowest cost solution available, until the maximum number of solutions
+			is reached.
+		)")
+	    .property<uint32_t>("max_solutions", "uint: maximum number of solutions that should be passed on")
+	    .def(py::init<const std::string&, Stage::pointer&&>(), "name"_a, "stage"_a);
+
 }
 }  // namespace python
 }  // namespace moveit

core/src/stages/CMakeLists.txt

@@ -13,6 +13,7 @@ add_library(${PROJECT_NAME}_stages
 	${PROJECT_INCLUDE}/stages/passthrough.h
 	${PROJECT_INCLUDE}/stages/noop.h
 	${PROJECT_INCLUDE}/stages/predicate_filter.h
+	${PROJECT_INCLUDE}/stages/limit_solutions.h
 
 	${PROJECT_INCLUDE}/stages/connect.h
 	${PROJECT_INCLUDE}/stages/move_to.h
@@ -34,6 +35,7 @@ add_library(${PROJECT_NAME}_stages
 	compute_ik.cpp
 	passthrough.cpp
 	predicate_filter.cpp
+	limit_solutions.cpp
 
 	connect.cpp
 	move_to.cpp

core/src/stages/limit_solutions.cpp

@@ -0,0 +1,78 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Bielefeld University nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Authors: Joseph Moore */
+
+#include <moveit/task_constructor/stages/limit_solutions.h>
+#include <moveit/task_constructor/storage.h>
+#include <moveit/task_constructor/cost_terms.h>
+#include <fmt/format.h>
+
+namespace moveit {
+namespace task_constructor {
+namespace stages {
+
+LimitSolutions::LimitSolutions(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) {
+	auto& p = properties();
+	p.declare<uint32_t>("max_solutions", "maximum number of solutions returned by this wrapper");
+	forwarded_solutions = 0;
+}
+
+void LimitSolutions::reset() {
+	upstream_solutions_.clear();
+	forwarded_solutions = 0;
+	WrapperBase::reset();
+}
+
+void LimitSolutions::onNewSolution(const SolutionBase& s) {
+	uint32_t max_solutions = properties().get<uint32_t>("max_solutions");
+	if (forwarded_solutions + upstream_solutions_.size() < max_solutions)
+		upstream_solutions_.push(&s);
+}
+
+bool LimitSolutions::canCompute() const {
+	return !upstream_solutions_.empty() || WrapperBase::canCompute();
+}
+
+void LimitSolutions::compute() {
+	if (WrapperBase::canCompute())
+		WrapperBase::compute();
+
+	if (upstream_solutions_.empty())
+		return;
+
+	++forwarded_solutions;
+	liftSolution(*upstream_solutions_.pop());
+}
+}  // namespace stages
+}  // namespace task_constructor
+}  // namespace moveit

core/test/CMakeLists.txt

@@ -36,6 +36,7 @@ if (CATKIN_ENABLE_TESTING)
 
 	mtc_add_gmock(test_mockups.cpp)
 	mtc_add_gtest(test_stage.cpp)
+	mtc_add_gtest(test_limit_solutions.cpp)
 	mtc_add_gtest(test_container.cpp)
 	mtc_add_gmock(test_serial.cpp)
 	mtc_add_gmock(test_pruning.cpp)

core/test/test_limit_solutions.cpp

@@ -0,0 +1,39 @@
+#include <moveit/task_constructor/stages/limit_solutions.h>
+
+#include "stage_mockups.h"
+
+using namespace moveit::task_constructor;
+
+using LimitSolutionsTest = TaskTestBase;
+
+TEST_F(LimitSolutionsTest, returnsMaxSolutions) {
+	auto g = std::make_unique<GeneratorMockup>(PredefinedCosts({ 2.0, 1.0, 3.0 }));
+	auto ls = std::make_unique<stages::LimitSolutions>("LimitSolutions", std::move(g));
+	ls->setMaxSolutions(2);
+	t.add(std::move(ls));
+
+	EXPECT_TRUE(t.plan());
+	EXPECT_EQ(t.solutions().size(), 2u);
+}
+
+TEST_F(LimitSolutionsTest, returnsAllChildSolutionsIfFewerThanMax) {
+	auto g = std::make_unique<GeneratorMockup>(PredefinedCosts({ 2.0, 1.0, 3.0 }));
+	auto ls = std::make_unique<stages::LimitSolutions>("LimitSolutions", std::move(g));
+	ls->setMaxSolutions(5);
+	t.add(std::move(ls));
+
+	EXPECT_TRUE(t.plan());
+	EXPECT_EQ(t.solutions().size(), 3u);
+}
+
+TEST_F(LimitSolutionsTest, returnsBestSolutions) {
+	// generator can create solutions with costs 1-4 in a single call to compute
+	auto g = std::make_unique<GeneratorMockup>(PredefinedCosts({ 4.0, 2.0, 3.0, 1.0 }), 4);
+	auto ls = std::make_unique<stages::LimitSolutions>("LimitSolutions", std::move(g));
+	ls->setMaxSolutions(3);  // however, only the first 3 solutions will be considered
+	t.add(std::move(ls));
+
+	EXPECT_TRUE(t.plan());
+	EXPECT_EQ(t.solutions().size(), 3u);  // only 3 solutions should have been generated
+	EXPECT_EQ(t.solutions().front()->cost(), 2.0);  // solution with cost 1.0 was not considered anymore
+}