Merge branch 'master' into ros2

Reverting d19b77782d and 717771e25a
ROS2 doesn't release pybind11 v3.
This commit is contained in:
Robert Haschke 2025-09-10 10:24:14 +02:00
commit 4472f3c38c
28 changed files with 387 additions and 59 deletions

2
.gitmodules vendored
View File

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

View File

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

View File

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

View File

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

View File

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

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -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");
}

View File

@ -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);
}

View 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

View File

@ -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());

View File

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

View File

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

View File

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

View File

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

View 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
}

View File

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

View File

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

View File

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

View File

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