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_place_pose.h"
|
||||||
#include "stages/generate_pose.h"
|
#include "stages/generate_pose.h"
|
||||||
#include "stages/generate_random_pose.h"
|
#include "stages/generate_random_pose.h"
|
||||||
|
#include "stages/limit_solutions.h"
|
||||||
#include "stages/modify_planning_scene.h"
|
#include "stages/modify_planning_scene.h"
|
||||||
#include "stages/move_relative.h"
|
#include "stages/move_relative.h"
|
||||||
#include "stages/move_to.h"
|
#include "stages/move_to.h"
|
||||||
|
#include "stages/passthrough.h"
|
||||||
#include "stages/predicate_filter.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(SimpleGraspBase)
|
||||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleGrasp)
|
PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleGrasp)
|
||||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleUnGrasp)
|
PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleUnGrasp)
|
||||||
|
PYBIND11_SMART_HOLDER_TYPE_CASTERS(LimitSolutions)
|
||||||
|
PYBIND11_SMART_HOLDER_TYPE_CASTERS(PassThrough)
|
||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace python {
|
namespace python {
|
||||||
@ -188,10 +190,6 @@ void export_stages(pybind11::module& m) {
|
|||||||
str: Default joint pose of the active group
|
str: Default joint pose of the active group
|
||||||
(defines cost of the inverse kinematics).
|
(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<uint32_t>("max_ik_solutions", "uint: max number of solutions to return")
|
||||||
.property<bool>("ignore_collisions", R"(
|
.property<bool>("ignore_collisions", R"(
|
||||||
bool: Specify if collisions with other members of
|
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")
|
.property<std::string>("grasp", "str: Name of the grasp pose")
|
||||||
.def(py::init<Stage::pointer&&, const std::string&>(), "pose_generator"_a,
|
.def(py::init<Stage::pointer&&, const std::string&>(), "pose_generator"_a,
|
||||||
"name"_a = std::string("ungrasp"));
|
"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 python
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
@ -13,6 +13,7 @@ add_library(${PROJECT_NAME}_stages
|
|||||||
${PROJECT_INCLUDE}/stages/passthrough.h
|
${PROJECT_INCLUDE}/stages/passthrough.h
|
||||||
${PROJECT_INCLUDE}/stages/noop.h
|
${PROJECT_INCLUDE}/stages/noop.h
|
||||||
${PROJECT_INCLUDE}/stages/predicate_filter.h
|
${PROJECT_INCLUDE}/stages/predicate_filter.h
|
||||||
|
${PROJECT_INCLUDE}/stages/limit_solutions.h
|
||||||
|
|
||||||
${PROJECT_INCLUDE}/stages/connect.h
|
${PROJECT_INCLUDE}/stages/connect.h
|
||||||
${PROJECT_INCLUDE}/stages/move_to.h
|
${PROJECT_INCLUDE}/stages/move_to.h
|
||||||
@ -34,6 +35,7 @@ add_library(${PROJECT_NAME}_stages
|
|||||||
compute_ik.cpp
|
compute_ik.cpp
|
||||||
passthrough.cpp
|
passthrough.cpp
|
||||||
predicate_filter.cpp
|
predicate_filter.cpp
|
||||||
|
limit_solutions.cpp
|
||||||
|
|
||||||
connect.cpp
|
connect.cpp
|
||||||
move_to.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