Add limit solutions stage type and bindings for PassThrough

This commit is contained in:
Joe Moore 2025-07-01 17:20:23 +01:00
parent 0cc398797f
commit 3f65390c8b
5 changed files with 163 additions and 4 deletions

View File

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

View File

@ -0,0 +1,64 @@
/*********************************************************************
* 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 = "PassThrough", Stage::pointer&& child = Stage::pointer());
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

View File

@ -64,6 +64,8 @@ PYBIND11_SMART_HOLDER_TYPE_CASTERS(Place)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleGraspBase)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleGrasp)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleUnGrasp)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(LimitSolutions)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(PassThrough)
namespace moveit {
namespace python {
@ -188,10 +190,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
@ -564,6 +562,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_<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", R"(
int: Set the maximum number of solutions that should be passed on.
)")
.def(py::init<const std::string&, Stage::pointer&&>(), "name"_a, "stage"_a);
properties::class_<PassThrough, Stage>(m, "PassThrough", R"(
Wrapper which simply passes on the solutions of a child stage..
)")
.def(py::init<const std::string&, Stage::pointer&&>(), "name"_a, "stage"_a);
}
} // namespace python
} // namespace moveit

View File

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

View File

@ -0,0 +1,76 @@
/*********************************************************************
* 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::onNewSolution(const SolutionBase& s) {
upstream_solutions_.push(&s);
}
bool LimitSolutions::canCompute() const {
const auto& props = properties();
uint32_t max_solutions = props.get<uint32_t>("max_solutions");
return (!upstream_solutions_.empty() || WrapperBase::canCompute())
&& forwarded_solutions < max_solutions;
}
void LimitSolutions::compute() {
if (WrapperBase::canCompute())
WrapperBase::compute();
if (upstream_solutions_.empty())
return;
const SolutionBase& s = *upstream_solutions_.pop();
forwarded_solutions += 1;
liftSolution(s);
}
} // namespace stages
} // namespace task_constructor
} // namespace moveit