mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
Add limit solutions stage type and bindings for PassThrough
This commit is contained in:
parent
0cc398797f
commit
3f65390c8b
@ -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"
|
||||
|
@ -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
|
@ -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
|
||||
|
@ -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
|
||||
|
76
core/src/stages/limit_solutions.cpp
Normal file
76
core/src/stages/limit_solutions.cpp
Normal 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
|
Loading…
Reference in New Issue
Block a user