mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
Merge branch 'master' into ros2
Revertingd19b77782d
and717771e25a
ROS2 doesn't release pybind11 v3.
This commit is contained in:
commit
4472f3c38c
2
.gitmodules
vendored
2
.gitmodules
vendored
@ -1,8 +1,6 @@
|
||||
[submodule "core/python/pybind11"]
|
||||
path = core/python/pybind11
|
||||
url = https://github.com/pybind/pybind11
|
||||
branch = smart_holder
|
||||
shallow = true
|
||||
[submodule "core/src/scope_guard"]
|
||||
path = core/src/scope_guard
|
||||
url = https://github.com/ricab/scope_guard
|
||||
|
@ -29,7 +29,7 @@ repos:
|
||||
- id: trailing-whitespace
|
||||
|
||||
- repo: https://github.com/psf/black
|
||||
rev: 24.10.0
|
||||
rev: 25.1.0
|
||||
hooks:
|
||||
- id: black
|
||||
args: ["--line-length", "100"]
|
||||
|
@ -44,6 +44,7 @@
|
||||
#include <moveit/robot_state/conversions.hpp>
|
||||
#include <moveit/utils/message_checks.hpp>
|
||||
#include <fmt/format.h>
|
||||
#include <fmt/ranges.h>
|
||||
|
||||
namespace {
|
||||
|
||||
|
@ -22,7 +22,7 @@ set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
set(CMAKE_CXX_EXTENSIONS OFF)
|
||||
|
||||
add_compile_options(-fvisibility-inlines-hidden)
|
||||
set(CMAKE_VISIBILITY_INLINES_HIDDEN TRUE)
|
||||
|
||||
set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor)
|
||||
|
||||
|
@ -59,7 +59,7 @@ class class_ : public pybind11::classh<type_, options...> // NOLINT(readability
|
||||
|
||||
public:
|
||||
// forward all constructors
|
||||
using base_class_::classh;
|
||||
using base_class_::base_class_;
|
||||
|
||||
template <typename PropertyType, typename... Extra>
|
||||
class_& property(const char* name, const Extra&... extra) {
|
||||
|
@ -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,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
|
@ -341,7 +341,7 @@ private:
|
||||
// name of the planner used to create this solution
|
||||
std::string planner_id_;
|
||||
// markers for this solution, e.g. target frame or collision indicators
|
||||
std::deque<visualization_msgs::msg::Marker> markers_;
|
||||
std::vector<visualization_msgs::msg::Marker> markers_;
|
||||
|
||||
// begin and end InterfaceState of this solution/trajectory
|
||||
const InterfaceState* start_ = nullptr;
|
||||
|
@ -45,6 +45,9 @@
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
#include <moveit/macros/class_forward.hpp>
|
||||
#include <moveit/robot_trajectory/robot_trajectory.hpp>
|
||||
#include <moveit/collision_detection/collision_common.hpp>
|
||||
#include <moveit/task_constructor/solvers/planner_interface.h>
|
||||
|
||||
namespace planning_scene {
|
||||
MOVEIT_CLASS_FORWARD(PlanningScene);
|
||||
@ -139,9 +142,23 @@ private:
|
||||
Int i;
|
||||
};
|
||||
|
||||
bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene,
|
||||
/** For a PoseStamped property, lookup the associated LinkModel* and yield the pose in global frame */
|
||||
bool getRobotTipForFrame(const Property& tip_pose, const planning_scene::PlanningScene& scene,
|
||||
const moveit::core::JointModelGroup* jmg, std::string& error_msg,
|
||||
const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame);
|
||||
|
||||
/** Add sphere markers to visualize given collisions */
|
||||
void addCollisionMarkers(std::vector<visualization_msgs::msg::Marker>& markers, const std::string& frame_id,
|
||||
const collision_detection::CollisionResult::ContactMap& contacts, double radius = 0.035);
|
||||
|
||||
/** Add sphere markers to visualize collisions within the trajectory */
|
||||
void addCollisionMarkers(std::vector<visualization_msgs::msg::Marker>& markers,
|
||||
const robot_trajectory::RobotTrajectory& trajectory,
|
||||
const planning_scene::PlanningSceneConstPtr& planning_scene);
|
||||
|
||||
/** Returns true if the result provides hints that planning failed due to collisions */
|
||||
bool hints_at_collisions(const solvers::PlannerInterface::Result& result);
|
||||
|
||||
} // namespace utils
|
||||
} // namespace task_constructor
|
||||
} // namespace moveit
|
||||
|
@ -1,18 +1,27 @@
|
||||
# moveit.task_constructor
|
||||
set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/task_constructor)
|
||||
pybind11_add_module(pymoveit_mtc
|
||||
add_library(${PROJECT_NAME}_python_tools SHARED
|
||||
${INCLUDES}/properties.h
|
||||
|
||||
src/properties.cpp
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME}_python_tools PUBLIC ${PROJECT_NAME}
|
||||
pybind11::pybind11 py_binding_tools::py_binding_tools)
|
||||
# Use minimum-size optimization for pybind11 bindings
|
||||
target_link_libraries(${PROJECT_NAME}_python_tools PUBLIC pybind11::opt_size)
|
||||
|
||||
# moveit.task_constructor
|
||||
pybind11_add_module(pymoveit_mtc
|
||||
src/solvers.cpp
|
||||
src/core.cpp
|
||||
src/stages.cpp
|
||||
src/module.cpp
|
||||
)
|
||||
target_include_directories(pymoveit_mtc PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>)
|
||||
target_link_libraries(pymoveit_mtc PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_stages py_binding_tools::py_binding_tools)
|
||||
target_link_libraries(pymoveit_mtc PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_stages ${PROJECT_NAME}_python_tools)
|
||||
|
||||
# install python libs
|
||||
# install libs
|
||||
install(TARGETS ${PROJECT_NAME}_python_tools
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib)
|
||||
install(TARGETS pymoveit_mtc
|
||||
LIBRARY DESTINATION "${PYTHON_INSTALL_DIR}"
|
||||
)
|
||||
|
@ -158,6 +158,7 @@ bool PropertyConverterBase::insert(const std::type_index& type_index, const std:
|
||||
return REGISTRY_SINGLETON.insert(type_index, ros_msg_name, to, from);
|
||||
}
|
||||
|
||||
__attribute__((visibility("default"))) // export this symbol as visible in the shared library
|
||||
void export_properties(py::module& m) {
|
||||
// clang-format off
|
||||
py::classh<Property>(m, "Property", "Holds an arbitrarily typed value and a default value")
|
||||
|
@ -37,6 +37,7 @@
|
||||
#include <moveit/task_constructor/stages.h>
|
||||
#include <moveit/task_constructor/stages/pick.h>
|
||||
#include <moveit/task_constructor/stages/simple_grasp.h>
|
||||
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
||||
#include <moveit/planning_scene/planning_scene.hpp>
|
||||
#include <moveit_msgs/msg/planning_scene.hpp>
|
||||
#include <pybind11/stl.h>
|
||||
@ -64,6 +65,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(PassThrough)
|
||||
PYBIND11_SMART_HOLDER_TYPE_CASTERS(LimitSolutions)
|
||||
|
||||
namespace moveit {
|
||||
namespace python {
|
||||
@ -188,10 +191,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
|
||||
@ -452,6 +451,7 @@ void export_stages(pybind11::module& m) {
|
||||
.property<std::string>("eef_parent_group", "str: Joint model group of the eef's parent")
|
||||
.def(py::init<Stage::pointer&&, const std::string&>(), "grasp_generator"_a,
|
||||
"name"_a = std::string("pick"))
|
||||
.def_property_readonly("cartesian_solver", &Pick::cartesianSolver)
|
||||
.def("setApproachMotion", &Pick::setApproachMotion, R"(
|
||||
The approaching motion towards the grasping state is represented
|
||||
by a twist message.
|
||||
@ -564,6 +564,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
|
||||
|
@ -15,19 +15,6 @@ def setUpModule():
|
||||
rclcpp.init()
|
||||
|
||||
|
||||
def pybind11_versions():
|
||||
try:
|
||||
keys = __builtins__.keys() # for use with pytest
|
||||
except AttributeError:
|
||||
keys = __builtins__.__dict__.keys() # use from cmdline
|
||||
return [k for k in keys if k.startswith("__pybind11_internals_v")]
|
||||
|
||||
|
||||
incompatible_pybind11_msg = "MoveIt and MTC use incompatible pybind11 versions: " + "\n- ".join(
|
||||
pybind11_versions()
|
||||
)
|
||||
|
||||
|
||||
class PyGenerator(core.Generator):
|
||||
"""Implements a custom 'Generator' stage."""
|
||||
|
||||
@ -110,18 +97,20 @@ class TestTrampolines(unittest.TestCase):
|
||||
return task
|
||||
|
||||
def plan(self, task, expected_solutions=None, wait=False):
|
||||
task.plan()
|
||||
try:
|
||||
task.plan()
|
||||
except TypeError as e:
|
||||
self.fail(f"{e}\nMoveIt and MTC use ABI-incompatible pybind11 versions")
|
||||
|
||||
if expected_solutions is not None:
|
||||
self.assertEqual(len(task.solutions), expected_solutions)
|
||||
if wait:
|
||||
input("Waiting for any key (allows inspection in rviz)")
|
||||
|
||||
@unittest.skipIf(len(pybind11_versions()) > 1, incompatible_pybind11_msg)
|
||||
def test_generator(self):
|
||||
task = self.create(PyGenerator())
|
||||
self.plan(task, expected_solutions=PyGenerator.max_calls)
|
||||
|
||||
@unittest.skipIf(len(pybind11_versions()) > 1, incompatible_pybind11_msg)
|
||||
def test_monitoring_generator(self):
|
||||
task = self.create(
|
||||
stages.CurrentState("current"),
|
||||
|
@ -254,6 +254,8 @@ class TestStages(unittest.TestCase):
|
||||
self._check(stage, "eef_frame", "eef_frame")
|
||||
self._check(stage, "eef_group", "eef_group")
|
||||
self._check(stage, "eef_parent_group", "eef_parent_group")
|
||||
# This will only work for pybind11 v3
|
||||
# self._check(stage.cartesian_solver, "max_velocity_scaling_factor", 0.1)
|
||||
|
||||
def test_Place(self):
|
||||
generator_stage = stages.GeneratePose("generator")
|
||||
|
@ -13,6 +13,7 @@ add_library(${PROJECT_NAME}_stages SHARED
|
||||
${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 SHARED
|
||||
compute_ik.cpp
|
||||
passthrough.cpp
|
||||
predicate_filter.cpp
|
||||
limit_solutions.cpp
|
||||
|
||||
connect.cpp
|
||||
move_to.cpp
|
||||
|
@ -96,7 +96,7 @@ void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string&
|
||||
struct IKSolution
|
||||
{
|
||||
std::vector<double> joint_positions;
|
||||
collision_detection::Contact contact;
|
||||
collision_detection::CollisionResult::ContactMap contacts;
|
||||
bool collision_free;
|
||||
bool satisfies_constraints;
|
||||
};
|
||||
@ -148,7 +148,7 @@ bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& sce
|
||||
}
|
||||
|
||||
std::string listCollisionPairs(const collision_detection::CollisionResult::ContactMap& contacts,
|
||||
const std::string& separator) {
|
||||
const std::string& separator = ", ") {
|
||||
std::string result;
|
||||
for (const auto& contact : contacts) {
|
||||
if (!result.empty())
|
||||
@ -352,8 +352,8 @@ void ComputeIK::compute() {
|
||||
generateCollisionMarkers(sandbox_state, appender, links_to_visualize);
|
||||
std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers()));
|
||||
solution.markAsFailure();
|
||||
// TODO: visualize collisions
|
||||
solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
|
||||
solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts));
|
||||
utils::addCollisionMarkers(solution.markers(), scene->getPlanningFrame(), collisions.contacts);
|
||||
auto colliding_scene{ scene->diff() };
|
||||
colliding_scene->setCurrentState(sandbox_state);
|
||||
spawn(InterfaceState(colliding_scene), std::move(solution));
|
||||
@ -402,9 +402,7 @@ void ComputeIK::compute() {
|
||||
req.group_name = jmg->getName();
|
||||
scene->checkCollision(req, res, *state);
|
||||
solution.collision_free = ignore_collisions || !res.collision;
|
||||
if (!res.contacts.empty()) {
|
||||
solution.contact = res.contacts.begin()->second.front();
|
||||
}
|
||||
solution.contacts = std::move(res.contacts);
|
||||
|
||||
return solution.satisfies_constraints && solution.collision_free;
|
||||
};
|
||||
@ -440,10 +438,8 @@ void ComputeIK::compute() {
|
||||
// compute cost as distance to compare_pose
|
||||
solution.setCost(s.cost() + jmg->distance(ik_solutions[i].joint_positions.data(), compare_pose.data()));
|
||||
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());
|
||||
solution.markAsFailure("Collision between " + listCollisionPairs(ik_solutions[i].contacts));
|
||||
utils::addCollisionMarkers(solution.markers(), scene->getPlanningFrame(), ik_solutions[i].contacts);
|
||||
} else if (!ik_solutions[i].satisfies_constraints) { // solution was violating constraints
|
||||
solution.markAsFailure("Constraints violated");
|
||||
}
|
||||
|
@ -158,6 +158,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
|
||||
intermediate_scenes.push_back(start);
|
||||
|
||||
bool success = false;
|
||||
bool has_potential_collisions = false;
|
||||
std::string comment = "No planners specified";
|
||||
std::vector<double> positions;
|
||||
for (const GroupPlannerVector::value_type& pair : planner_) {
|
||||
@ -177,6 +178,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
|
||||
|
||||
if (!success) {
|
||||
comment = result.message;
|
||||
has_potential_collisions = trajectory && utils::hints_at_collisions(result);
|
||||
break;
|
||||
}
|
||||
|
||||
@ -195,8 +197,15 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
|
||||
solution = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
|
||||
if (!solution) // success == false or merging failed: store sequentially
|
||||
solution = makeSequential(sub_trajectories, intermediate_scenes, from, to);
|
||||
if (!success) // error during sequential planning
|
||||
if (!success) { // error already during sequential planning
|
||||
solution->markAsFailure(comment);
|
||||
if (has_potential_collisions) {
|
||||
// add collision markers for last (failed) trajectory segment
|
||||
auto sequence = std::dynamic_pointer_cast<SolutionSequence>(solution);
|
||||
auto trajectory = dynamic_cast<const SubTrajectory*>(sequence->solutions().back())->trajectory();
|
||||
utils::addCollisionMarkers(solution->markers(), *trajectory, start);
|
||||
}
|
||||
}
|
||||
connect(from, to, solution);
|
||||
}
|
||||
|
||||
|
78
core/src/stages/limit_solutions.cpp
Normal file
78
core/src/stages/limit_solutions.cpp
Normal file
@ -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
|
@ -156,6 +156,7 @@ std::pair<InterfaceState, SubTrajectory> ModifyPlanningScene::apply(const Interf
|
||||
if (res.collision) {
|
||||
const auto contact = res.contacts.begin()->second.front();
|
||||
traj.markAsFailure(contact.body_name_1 + " colliding with " + contact.body_name_2);
|
||||
utils::addCollisionMarkers(traj.markers(), scene->getPlanningFrame(), res.contacts);
|
||||
}
|
||||
} catch (const std::exception& e) {
|
||||
traj.markAsFailure(e.what());
|
||||
|
@ -100,7 +100,6 @@ static bool getJointStateFromOffset(const boost::any& direction, const Interface
|
||||
throw std::runtime_error("Cannot plan for multi-variable joint '" + j.first + "'");
|
||||
auto index = jm->getFirstVariableIndex();
|
||||
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + sign * j.second);
|
||||
robot_state.enforceBounds(jm);
|
||||
}
|
||||
robot_state.update();
|
||||
return true;
|
||||
@ -112,7 +111,7 @@ static bool getJointStateFromOffset(const boost::any& direction, const Interface
|
||||
}
|
||||
|
||||
// Create an arrow marker from start_pose to reached_pose, split into a red and green part based on achieved distance
|
||||
static void visualizePlan(std::deque<visualization_msgs::msg::Marker>& markers, Interface::Direction dir, bool success,
|
||||
static void visualizePlan(std::vector<visualization_msgs::msg::Marker>& markers, Interface::Direction dir, bool success,
|
||||
const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& start_pose,
|
||||
const Eigen::Isometry3d& reached_pose, const Eigen::Vector3d& linear, double distance) {
|
||||
double linear_norm = linear.norm();
|
||||
@ -200,14 +199,17 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
|
||||
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
|
||||
bool success = false;
|
||||
bool has_potential_collisions = false;
|
||||
std::string comment = "";
|
||||
|
||||
if (getJointStateFromOffset(direction, dir, jmg, scene->getCurrentStateNonConst())) {
|
||||
// plan to joint-space target
|
||||
auto result = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
|
||||
success = bool(result);
|
||||
if (!success)
|
||||
if (!success) {
|
||||
comment = result.message;
|
||||
has_potential_collisions = robot_trajectory && utils::hints_at_collisions(result);
|
||||
}
|
||||
solution.setPlannerId(planner_->getPlannerId());
|
||||
} else {
|
||||
// Cartesian targets require an IK reference frame
|
||||
@ -301,8 +303,10 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
auto result =
|
||||
planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
||||
success = bool(result);
|
||||
if (!success)
|
||||
if (!success) {
|
||||
comment = result.message;
|
||||
has_potential_collisions = robot_trajectory && utils::hints_at_collisions(result);
|
||||
}
|
||||
solution.setPlannerId(planner_->getPlannerId());
|
||||
|
||||
if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) { // the following requires a robot_trajectory
|
||||
@ -347,8 +351,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
robot_trajectory->reverse();
|
||||
solution.setTrajectory(robot_trajectory);
|
||||
|
||||
if (!success)
|
||||
if (!success) {
|
||||
solution.markAsFailure(comment);
|
||||
if (has_potential_collisions)
|
||||
utils::addCollisionMarkers(solution.markers(), *robot_trajectory, scene);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
@ -205,14 +205,17 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
||||
const auto& path_constraints = props.get<moveit_msgs::msg::Constraints>("path_constraints");
|
||||
robot_trajectory::RobotTrajectoryPtr robot_trajectory;
|
||||
bool success = false;
|
||||
bool has_potential_collisions = false;
|
||||
std::string comment = "";
|
||||
|
||||
if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) {
|
||||
// plan to joint-space target
|
||||
auto result = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
|
||||
success = bool(result);
|
||||
if (!success)
|
||||
if (!success) {
|
||||
comment = result.message;
|
||||
has_potential_collisions = robot_trajectory && utils::hints_at_collisions(result);
|
||||
}
|
||||
solution.setPlannerId(planner_->getPlannerId());
|
||||
} else { // Cartesian goal
|
||||
// Where to go?
|
||||
@ -252,8 +255,10 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
||||
const auto result =
|
||||
planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints);
|
||||
success = bool(result);
|
||||
if (!success)
|
||||
if (!success) {
|
||||
comment = result.message;
|
||||
has_potential_collisions = robot_trajectory && utils::hints_at_collisions(result);
|
||||
}
|
||||
solution.setPlannerId(planner_->getPlannerId());
|
||||
}
|
||||
|
||||
@ -269,9 +274,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
||||
robot_trajectory->reverse();
|
||||
solution.setTrajectory(robot_trajectory);
|
||||
|
||||
if (!success)
|
||||
if (!success) {
|
||||
solution.markAsFailure(comment);
|
||||
|
||||
if (has_potential_collisions)
|
||||
utils::addCollisionMarkers(solution.markers(), *robot_trajectory, scene);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
@ -43,6 +43,8 @@
|
||||
|
||||
#include <moveit/robot_state/robot_state.hpp>
|
||||
#include <moveit/planning_scene/planning_scene.hpp>
|
||||
#include <moveit/robot_trajectory/robot_trajectory.hpp>
|
||||
#include <moveit/utils/moveit_error_code.hpp>
|
||||
|
||||
#include <moveit/task_constructor/properties.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
@ -51,7 +53,7 @@ namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace utils {
|
||||
|
||||
bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene,
|
||||
bool getRobotTipForFrame(const Property& tip_pose, const planning_scene::PlanningScene& scene,
|
||||
const moveit::core::JointModelGroup* jmg, std::string& error_msg,
|
||||
const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame) {
|
||||
auto get_tip = [&jmg]() -> const moveit::core::LinkModel* {
|
||||
@ -66,7 +68,7 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin
|
||||
|
||||
error_msg = "";
|
||||
|
||||
if (property.value().empty()) { // property undefined
|
||||
if (tip_pose.value().empty()) { // property undefined
|
||||
robot_link = get_tip();
|
||||
if (!robot_link) {
|
||||
error_msg = "missing ik_frame";
|
||||
@ -74,7 +76,7 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin
|
||||
}
|
||||
tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link);
|
||||
} else {
|
||||
auto ik_pose_msg = boost::any_cast<geometry_msgs::msg::PoseStamped>(property.value());
|
||||
auto ik_pose_msg = boost::any_cast<geometry_msgs::msg::PoseStamped>(tip_pose.value());
|
||||
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
|
||||
|
||||
robot_link = nullptr;
|
||||
@ -86,7 +88,7 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin
|
||||
error_msg = ss.str();
|
||||
return false;
|
||||
}
|
||||
if (!robot_link)
|
||||
if (!robot_link) // if ik_pose_msg.header.frame_id was empty, use default tip frame
|
||||
robot_link = get_tip();
|
||||
if (!robot_link) {
|
||||
error_msg = "ik_frame doesn't specify a link frame";
|
||||
@ -101,6 +103,59 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin
|
||||
return true;
|
||||
}
|
||||
|
||||
void addCollisionMarkers(std::vector<visualization_msgs::msg::Marker>& markers, const std::string& frame_id,
|
||||
const collision_detection::CollisionResult::ContactMap& contacts, double radius) {
|
||||
visualization_msgs::msg::Marker m;
|
||||
m.header.frame_id = frame_id;
|
||||
m.ns = "collisions";
|
||||
m.type = visualization_msgs::msg::Marker::SPHERE;
|
||||
m.action = visualization_msgs::msg::Marker::ADD;
|
||||
m.pose.orientation.x = 0.0;
|
||||
m.pose.orientation.y = 0.0;
|
||||
m.pose.orientation.z = 0.0;
|
||||
m.pose.orientation.w = 1.0;
|
||||
m.scale.x = m.scale.y = m.scale.z = radius * 2.0;
|
||||
m.color.r = m.color.a = 1.0;
|
||||
|
||||
for (const auto& collision : contacts) {
|
||||
for (const auto& contact : collision.second) {
|
||||
m.pose.position.x = contact.pos.x();
|
||||
m.pose.position.y = contact.pos.y();
|
||||
m.pose.position.z = contact.pos.z();
|
||||
markers.push_back(m);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void addCollisionMarkers(std::vector<visualization_msgs::msg::Marker>& markers,
|
||||
const robot_trajectory::RobotTrajectory& trajectory,
|
||||
const planning_scene::PlanningSceneConstPtr& planning_scene) {
|
||||
std::size_t n_wp = trajectory.getWayPointCount();
|
||||
for (std::size_t it = 0; it < n_wp; ++it) {
|
||||
const moveit::core::RobotState& robot_state = trajectory.getWayPoint(it);
|
||||
|
||||
// Collision contact check
|
||||
collision_detection::CollisionRequest req;
|
||||
collision_detection::CollisionResult res;
|
||||
req.contacts = true;
|
||||
req.max_contacts = 10;
|
||||
|
||||
planning_scene->checkCollision(req, res, robot_state);
|
||||
|
||||
if (res.contact_count > 0)
|
||||
addCollisionMarkers(markers, planning_scene->getPlanningFrame(), res.contacts);
|
||||
}
|
||||
}
|
||||
|
||||
bool hints_at_collisions(const solvers::PlannerInterface::Result& result) {
|
||||
static const std::string INVALID_MOTION_PLAN_MSG = []() {
|
||||
moveit_msgs::msg::MoveItErrorCodes err;
|
||||
err.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
|
||||
return moveit::core::errorCodeToString(err);
|
||||
}();
|
||||
return result.message == INVALID_MOTION_PLAN_MSG;
|
||||
}
|
||||
|
||||
} // namespace utils
|
||||
} // namespace task_constructor
|
||||
} // namespace moveit
|
||||
|
@ -44,6 +44,7 @@ if (BUILD_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)
|
||||
|
39
core/test/test_limit_solutions.cpp
Normal file
39
core/test/test_limit_solutions.cpp
Normal file
@ -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
|
||||
}
|
@ -4,6 +4,7 @@
|
||||
#include <moveit/task_constructor/stages/move_relative.h>
|
||||
#include <moveit/task_constructor/stages/fixed_state.h>
|
||||
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
||||
#include <moveit/task_constructor/solvers/joint_interpolation.h>
|
||||
#include <moveit/task_constructor/solvers/pipeline_planner.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.hpp>
|
||||
@ -32,6 +33,11 @@ solvers::CartesianPathPtr create<solvers::CartesianPath>(const rclcpp::Node::Sha
|
||||
return std::make_shared<solvers::CartesianPath>();
|
||||
}
|
||||
template <>
|
||||
solvers::JointInterpolationPlannerPtr
|
||||
create<solvers::JointInterpolationPlanner>(const rclcpp::Node::SharedPtr& /*unused*/) {
|
||||
return std::make_shared<solvers::JointInterpolationPlanner>();
|
||||
}
|
||||
template <>
|
||||
solvers::PipelinePlannerPtr create<solvers::PipelinePlanner>(const rclcpp::Node::SharedPtr& node) {
|
||||
auto p = std::make_shared<solvers::PipelinePlanner>(node, "pilz_industrial_motion_planner", "LIN");
|
||||
p->setProperty("max_velocity_scaling_factor", 0.1);
|
||||
@ -67,6 +73,7 @@ struct PandaMoveRelative : public testing::Test
|
||||
}
|
||||
};
|
||||
using PandaMoveRelativeCartesian = PandaMoveRelative<solvers::CartesianPath>;
|
||||
using PandaMoveRelativeJoint = PandaMoveRelative<solvers::JointInterpolationPlanner>;
|
||||
|
||||
moveit_msgs::msg::CollisionObject createObject(const std::string& id, const geometry_msgs::msg::Pose& pose) {
|
||||
moveit_msgs::msg::CollisionObject co;
|
||||
@ -162,6 +169,25 @@ TEST_F(PandaMoveRelativeCartesian, cartesianRotateAttachedIKFrame) {
|
||||
EXPECT_CONST_POSITION(move->solutions().front(), attached_object);
|
||||
}
|
||||
|
||||
// https://github.com/moveit/moveit_task_constructor/issues/664
|
||||
TEST_F(PandaMoveRelativeJoint, jointOutsideBound) {
|
||||
// move joint inside limit
|
||||
auto initial_jpos = scene->getCurrentState().getJointPositions("panda_joint7");
|
||||
move->setDirection([initial_jpos] {
|
||||
return std::map<std::string, double>{ { "panda_joint7", 2.0 - *initial_jpos } };
|
||||
}());
|
||||
EXPECT_TRUE(this->t.plan()) << "Plan should succeed, joint inside limit";
|
||||
|
||||
this->t.reset();
|
||||
|
||||
// move joint outside limit: 2.8973
|
||||
move->setDirection([initial_jpos] {
|
||||
return std::map<std::string, double>{ { "panda_joint7", 3.0 - *initial_jpos } };
|
||||
}());
|
||||
|
||||
EXPECT_FALSE(this->t.plan()) << "Plan should fail, joint outside limit";
|
||||
}
|
||||
|
||||
using PlannerTypes = ::testing::Types<solvers::CartesianPath, solvers::PipelinePlanner>;
|
||||
TYPED_TEST_SUITE(PandaMoveRelative, PlannerTypes);
|
||||
TYPED_TEST(PandaMoveRelative, cartesianCollisionMinMaxDistance) {
|
||||
|
@ -17,6 +17,9 @@
|
||||
<depend>moveit_ros_planning_interface</depend>
|
||||
<depend>moveit_task_constructor_core</depend>
|
||||
<exec_depend>moveit_configs_utils</exec_depend>
|
||||
|
||||
<exec_depend>moveit_task_constructor_capabilities</exec_depend>
|
||||
<exec_depend>moveit_task_constructor_visualization</exec_depend>
|
||||
<exec_depend>moveit_resources_panda_moveit_config</exec_depend>
|
||||
<exec_depend>moveit_task_constructor_capabilities</exec_depend>
|
||||
<exec_depend>py_binding_tools</exec_depend>
|
||||
|
@ -98,6 +98,8 @@ approach.header.frame_id = "world"
|
||||
approach.twist.linear.z = -1.0
|
||||
pick.setApproachMotion(approach, 0.03, 0.1)
|
||||
|
||||
pick.cartesian_solver.max_velocity_scaling_factor = 0.1
|
||||
|
||||
# Twist to lift the object
|
||||
lift = TwistStamped()
|
||||
lift.header.frame_id = "panda_hand"
|
||||
|
@ -173,8 +173,9 @@ bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_t
|
||||
***************************************************/
|
||||
// Connect initial open-hand state with pre-grasp pose defined in the following
|
||||
{
|
||||
auto stage = std::make_unique<stages::Connect>(
|
||||
"move to pick", stages::Connect::GroupPlannerVector{ { params.arm_group_name, sampling_planner } });
|
||||
stages::Connect::GroupPlannerVector planners = { { params.arm_group_name, sampling_planner },
|
||||
{ params.hand_group_name, sampling_planner } };
|
||||
auto stage = std::make_unique<stages::Connect>("move to pick", planners);
|
||||
stage->setTimeout(5.0);
|
||||
stage->properties().configureInitFrom(Stage::PARENT);
|
||||
t.add(std::move(stage));
|
||||
|
Loading…
Reference in New Issue
Block a user