From 2e9a223827b60b316b3d2c763ce5201e7b09d377 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 16 Jul 2024 08:24:55 +0200 Subject: [PATCH 01/10] MoveRelative: handle equal min/max distance (#593) When min_distance == max_distance > 0.0, the minimal distance might be missed due to numerical errors. To avoid this, deactivate the minimal distance check and run the full distance as given by max_distance. --- core/src/stages/move_relative.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 3f1b915e..634c8b56 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -185,6 +185,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning double max_distance = props.get("max_distance"); double min_distance = props.get("min_distance"); + + // if min_distance == max_distance, resort to moving full distance (negative min_distance) + if (max_distance > 0.0 && std::fabs(max_distance - min_distance) < std::numeric_limits::epsilon()) + min_distance = -1.0; + const auto& path_constraints = props.get("path_constraints"); robot_trajectory::RobotTrajectoryPtr robot_trajectory; From 60ccd744435e90393a38bb987bb398f8f9760974 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 16 Jul 2024 09:18:16 -0600 Subject: [PATCH 02/10] MoveRelative: fix segfault on empty trajectory (#595) Check that at least one robot state exists in the robot trajectory before accessing it. Signed-off-by: Paul Gesel --- core/src/stages/move_relative.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 634c8b56..7bf065f3 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -296,7 +296,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning if (!success) comment = result.message; - if (robot_trajectory) { // the following requires a robot_trajectory returned from planning + if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) { // the following requires a robot_trajectory + // returned from planning moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr(); reached_state->updateLinkTransforms(); const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset; @@ -331,7 +332,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning } // store result - if (robot_trajectory) { + if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) { scene->setCurrentState(robot_trajectory->getLastWayPoint()); if (dir == Interface::BACKWARD) robot_trajectory->reverse(); From cd28bdcd1f72af5a713a823c0da287dedfc0cc48 Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Wed, 17 Jul 2024 08:56:01 -0400 Subject: [PATCH 03/10] Fix early planning preemption (#597) Calling preempt() before plan() is able to reset the preempt_requested_ flag causes the preemption request to get lost. To avoid this issue, we allow a) manual resetting of the request and b) reset the request before leaving plan(). --- .gitmodules | 3 ++ core/include/moveit/task_constructor/task.h | 3 +- core/include/moveit/task_constructor/task_p.h | 2 +- core/src/scope_guard | 1 + core/src/task.cpp | 10 ++++++- core/test/test_container.cpp | 28 +++++++++++++++++++ 6 files changed, 44 insertions(+), 3 deletions(-) create mode 160000 core/src/scope_guard diff --git a/.gitmodules b/.gitmodules index 9339bbb2..d7508c72 100644 --- a/.gitmodules +++ b/.gitmodules @@ -3,3 +3,6 @@ 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 diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 00ea354c..d1c12374 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -127,8 +127,9 @@ public: /// reset, init scene (if not yet done), and init all stages, then start planning moveit::core::MoveItErrorCode plan(size_t max_solutions = 0); - /// interrupt current planning (or execution) + /// interrupt current planning void preempt(); + void resetPreemptRequest(); /// execute solution, return the result moveit::core::MoveItErrorCode execute(const SolutionBase& s); diff --git a/core/include/moveit/task_constructor/task_p.h b/core/include/moveit/task_constructor/task_p.h index f0e6dc0a..ef42dd34 100644 --- a/core/include/moveit/task_constructor/task_p.h +++ b/core/include/moveit/task_constructor/task_p.h @@ -63,7 +63,7 @@ private: std::string ns_; robot_model_loader::RobotModelLoaderPtr robot_model_loader_; moveit::core::RobotModelConstPtr robot_model_; - bool preempt_requested_; + std::atomic preempt_requested_; // introspection and monitoring std::unique_ptr introspection_; diff --git a/core/src/scope_guard b/core/src/scope_guard new file mode 160000 index 00000000..71a04528 --- /dev/null +++ b/core/src/scope_guard @@ -0,0 +1 @@ +Subproject commit 71a04528184db1749dd08ebbbf4daf3b5dca21fd diff --git a/core/src/task.cpp b/core/src/task.cpp index f969cf9e..dbba7389 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -46,6 +46,8 @@ #include #include +#include + #include namespace { @@ -234,6 +236,9 @@ void Task::compute() { } moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) { + // ensure the preempt request is resetted once this method exits + auto guard = sg::make_scope_guard([this]() noexcept { this->resetPreemptRequest(); }); + auto impl = pimpl(); init(); @@ -245,7 +250,6 @@ moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) { explainFailure(); return error_code; }; - impl->preempt_requested_ = false; const double available_time = timeout(); const auto start_time = std::chrono::steady_clock::now(); while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) { @@ -266,6 +270,10 @@ void Task::preempt() { pimpl()->preempt_requested_ = true; } +void Task::resetPreemptRequest() { + pimpl()->preempt_requested_ = false; +} + moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) { actionlib::SimpleActionClient ac("execute_task_solution"); if (!ac.waitForServer(ros::Duration(0.5))) { diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 876a1e6d..3746c239 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -672,3 +672,31 @@ TEST(Task, timeout) { EXPECT_TRUE(t.plan()); EXPECT_EQ(t.solutions().size(), 2u); } + +// https://github.com/moveit/moveit_task_constructor/pull/597 +// start planning in another thread, then preempt it in this thread +TEST(Task, preempt) { + moveit::core::MoveItErrorCode ec; + resetMockupIds(); + + Task t; + t.setRobotModel(getModel()); + + auto timeout = std::chrono::milliseconds(10); + t.add(std::make_unique(PredefinedCosts::constant(0.0))); + t.add(std::make_unique(timeout)); + + // preempt before preempt_request_ is reset in plan() + { + std::thread thread{ [&ec, &t, timeout] { + std::this_thread::sleep_for(timeout); + ec = t.plan(1); + } }; + t.preempt(); + thread.join(); + } + + EXPECT_EQ(ec, moveit::core::MoveItErrorCode::PREEMPTED); + EXPECT_EQ(t.solutions().size(), 0u); + EXPECT_TRUE(t.plan(1)); // make sure the preempt request has been resetted on the previous call to plan() +} From 763148664838f6920304d57d026db5ccfc0fee91 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 17 Jul 2024 15:10:53 +0200 Subject: [PATCH 04/10] Add unittest for #581 --- core/test/test_fallback.cpp | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/core/test/test_fallback.cpp b/core/test/test_fallback.cpp index 87f9368f..c5b7eb90 100644 --- a/core/test/test_fallback.cpp +++ b/core/test/test_fallback.cpp @@ -1,7 +1,8 @@ #include #include #include -#include +#include +#include #include #include "stage_mockups.h" @@ -159,6 +160,26 @@ TEST_F(FallbacksFixturePropagate, activeChildReset) { EXPECT_COSTS(fwd2->solutions(), testing::IsEmpty()); } +// https://github.com/moveit/moveit_task_constructor/issues/581#issuecomment-2147985474 +TEST_F(FallbacksFixturePropagate, filterPropagatesFailures) { + t.add(std::make_unique(PredefinedCosts::single(0.0))); + + auto fallbacks = std::make_unique("Fallbacks"); + auto add_filtered_fwd = [&fallbacks](double cost, bool accept) { + auto fwd = std::make_unique(PredefinedCosts::constant(cost)); + auto filter = std::make_unique("filter", std::move(fwd)); + filter->setPredicate([accept](const SolutionBase& /*solution*/, std::string& /*comment*/) { return accept; }); + fallbacks->add(std::move(filter)); + }; + add_filtered_fwd(INF, false); // Propagate fails, filter rejects + add_filtered_fwd(2.0, true); // Propagate succeeds, filter accepts + fallbacks->add(std::make_unique()); + t.add(std::move(fallbacks)); + + EXPECT_TRUE(t.plan()); + EXPECT_COSTS(t.solutions(), testing::ElementsAre(2.)); +} + using FallbacksFixtureConnect = TaskTestBase; TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) { From 4f69a22ddb41835af6dc8578408f651b7a3e5ecc Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 18 Jul 2024 13:45:15 +0200 Subject: [PATCH 05/10] Silent error "Found empty JointState message" --- capabilities/src/execute_task_solution_capability.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 2a3afa9e..17facf74 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -170,8 +170,10 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr exec_traj.effect_on_success_ = [this, &scene_diff = const_cast<::moveit_msgs::PlanningScene&>(sub_traj.scene_diff), description](const plan_execution::ExecutableMotionPlan* /*plan*/) { + // Never modify joint state directly (only via robot trajectories) scene_diff.robot_state.joint_state = sensor_msgs::JointState(); scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::MultiDOFJointState(); + scene_diff.robot_state.is_diff = true; // silent empty JointState msg error if (!moveit::core::isEmpty(scene_diff)) { ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description); From fdc06c3b9105dadec2f44ee3e4b3133986945e86 Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Fri, 19 Jul 2024 09:46:12 -0400 Subject: [PATCH 06/10] Reduce stop time due to preempt (#598) The preempt_request_ flag was only checked at the top-level task container before each compute iteration. As a single sweep might take a while, we should check the flag before computing each stage. --- .../include/moveit/task_constructor/stage_p.h | 18 +++++++++++ core/src/stage.cpp | 3 +- core/src/task.cpp | 11 +++++-- core/test/stage_mockups.h | 4 +++ core/test/test_container.cpp | 31 ++++++++++++++----- 5 files changed, 56 insertions(+), 11 deletions(-) diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 5d359b5c..b965ac19 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -57,6 +57,13 @@ namespace moveit { namespace task_constructor { +/// exception thrown by StagePrivate::runCompute() +class PreemptStageException : public std::exception +{ +public: + explicit PreemptStageException() {} +}; + class ContainerBase; class StagePrivate { @@ -146,6 +153,10 @@ public: bool storeFailures() const { return introspection_ != nullptr; } void runCompute() { ROS_DEBUG_STREAM_NAMED("Stage", fmt::format("Computing stage '{}'", name())); + + if (preempted()) + throw PreemptStageException(); + auto compute_start_time = std::chrono::steady_clock::now(); try { compute(); @@ -159,6 +170,11 @@ public: /** compute cost for solution through configured CostTerm */ void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution); + void setPreemptRequestedMember(const std::atomic* preempt_requested) { + preempt_requested_ = preempt_requested; + } + bool preempted() const { return preempt_requested_ != nullptr && *preempt_requested_; } + protected: StagePrivate& operator=(StagePrivate&& other); @@ -197,6 +213,8 @@ private: InterfaceWeakPtr next_starts_; // interface to be used for sendForward() Introspection* introspection_; // task's introspection instance + + const std::atomic* preempt_requested_; }; PIMPL_FUNCTIONS(Stage) std::ostream& operator<<(std::ostream& os, const StagePrivate& stage); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 97c99826..9fbf4d27 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -102,7 +102,8 @@ StagePrivate::StagePrivate(Stage* me, const std::string& name) , cost_term_{ std::make_unique() } , total_compute_time_{} , parent_{ nullptr } - , introspection_{ nullptr } {} + , introspection_{ nullptr } + , preempt_requested_{ nullptr } {} StagePrivate& StagePrivate::operator=(StagePrivate&& other) { assert(typeid(*this) == typeid(other)); diff --git a/core/src/task.cpp b/core/src/task.cpp index dbba7389..ab375d2c 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -213,11 +213,12 @@ void Task::init() { // task expects its wrapped child to push to both ends, this triggers interface resolution stages()->pimpl()->resolveInterface(InterfaceFlags({ GENERATE })); - // provide introspection instance to all stages + // provide introspection instance and preempt_requested to all stages auto* introspection = impl->introspection_.get(); impl->traverseStages( - [introspection](Stage& stage, int /*depth*/) { + [introspection, impl](Stage& stage, int /*depth*/) { stage.pimpl()->setIntrospection(introspection); + stage.pimpl()->setPreemptRequestedMember(&impl->preempt_requested_); return true; }, 1, UINT_MAX); @@ -232,7 +233,11 @@ bool Task::canCompute() const { } void Task::compute() { - stages()->pimpl()->runCompute(); + try { + stages()->pimpl()->runCompute(); + } catch (const PreemptStageException& e) { + // do nothing, needed for early stop + } } moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) { diff --git a/core/test/stage_mockups.h b/core/test/stage_mockups.h index fc75c633..35d01e65 100644 --- a/core/test/stage_mockups.h +++ b/core/test/stage_mockups.h @@ -65,6 +65,7 @@ struct GeneratorMockup : public Generator void init(const moveit::core::RobotModelConstPtr& robot_model) override; bool canCompute() const override; void compute() override; + virtual void reset() override { runs_ = 0; }; }; struct MonitoringGeneratorMockup : public MonitoringGenerator @@ -81,6 +82,7 @@ struct MonitoringGeneratorMockup : public MonitoringGenerator bool canCompute() const override { return false; } void compute() override {} void onNewSolution(const SolutionBase& s) override; + virtual void reset() override { runs_ = 0; }; }; struct ConnectMockup : public Connecting @@ -97,6 +99,7 @@ struct ConnectMockup : public Connecting using Connecting::compatible; // make this accessible for testing void compute(const InterfaceState& from, const InterfaceState& to) override; + virtual void reset() override { runs_ = 0; }; }; struct PropagatorMockup : public PropagatingEitherWay @@ -113,6 +116,7 @@ struct PropagatorMockup : public PropagatingEitherWay void computeForward(const InterfaceState& from) override; void computeBackward(const InterfaceState& to) override; + virtual void reset() override { runs_ = 0; }; }; struct ForwardMockup : public PropagatorMockup diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 3746c239..5d7b722c 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -674,21 +674,20 @@ TEST(Task, timeout) { } // https://github.com/moveit/moveit_task_constructor/pull/597 +// https://github.com/moveit/moveit_task_constructor/pull/598 // start planning in another thread, then preempt it in this thread -TEST(Task, preempt) { +TEST_F(TaskTestBase, preempt) { moveit::core::MoveItErrorCode ec; resetMockupIds(); - Task t; - t.setRobotModel(getModel()); - auto timeout = std::chrono::milliseconds(10); - t.add(std::make_unique(PredefinedCosts::constant(0.0))); - t.add(std::make_unique(timeout)); + auto gen1 = add(t, new GeneratorMockup(PredefinedCosts::constant(0.0))); + auto fwd1 = add(t, new TimedForwardMockup(timeout)); + auto fwd2 = add(t, new TimedForwardMockup(timeout)); // preempt before preempt_request_ is reset in plan() { - std::thread thread{ [&ec, &t, timeout] { + std::thread thread{ [&ec, this, timeout] { std::this_thread::sleep_for(timeout); ec = t.plan(1); } }; @@ -698,5 +697,23 @@ TEST(Task, preempt) { EXPECT_EQ(ec, moveit::core::MoveItErrorCode::PREEMPTED); EXPECT_EQ(t.solutions().size(), 0u); + EXPECT_EQ(gen1->runs_, 0u); + EXPECT_EQ(fwd1->runs_, 0u); + EXPECT_EQ(fwd2->runs_, 0u); + EXPECT_TRUE(t.plan(1)); // make sure the preempt request has been resetted on the previous call to plan() + + t.reset(); + { + std::thread thread{ [&ec, this] { ec = t.plan(1); } }; + std::this_thread::sleep_for(timeout / 2.0); + t.preempt(); + thread.join(); + } + + EXPECT_EQ(ec, moveit::core::MoveItErrorCode::PREEMPTED); + EXPECT_EQ(t.solutions().size(), 0u); + EXPECT_EQ(gen1->runs_, 1u); + EXPECT_EQ(fwd1->runs_, 1u); + EXPECT_EQ(fwd2->runs_, 0u); EXPECT_TRUE(t.plan(1)); // make sure the preempt request has been resetted on the previous call to plan() } From 237a07f96f1280e182a37b2b48347e3ea2d66e49 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 2 Sep 2024 09:25:12 +0200 Subject: [PATCH 07/10] CI: Update actions --- .github/workflows/ci.yaml | 5 ++--- .pre-commit-config.yaml | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 1ee865b5..1feef010 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -56,15 +56,14 @@ jobs: submodules: recursive - name: Cache ccache - uses: rhaschke/cache@main + uses: actions/cache@v4 with: + save-always: true path: ${{ env.CCACHE_DIR }} key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} restore-keys: | ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} ccache-${{ env.CACHE_PREFIX }} - env: - GHA_CACHE_SAVE: always - id: ici name: Run industrial_ci diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index cff5e8c0..6557cc43 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -29,7 +29,7 @@ repos: - id: trailing-whitespace - repo: https://github.com/psf/black - rev: 24.4.2 + rev: 24.8.0 hooks: - id: black args: ["--line-length", "100"] From 184d39897ae28158e57800535f50ace93d0a1016 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 2 Sep 2024 11:20:45 +0200 Subject: [PATCH 08/10] Add missing test dependency --- core/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/core/package.xml b/core/package.xml index 20c9fd8a..8286b632 100644 --- a/core/package.xml +++ b/core/package.xml @@ -34,6 +34,7 @@ rostest moveit_resources_fanuc_moveit_config moveit_planners + moveit_commander From 5a4480814628db4207d9734d63dd907619ced93b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Mon, 2 Sep 2024 22:52:47 +0200 Subject: [PATCH 09/10] clean up dependencies for rviz_marker_tools (#610) liburdfdom-dev is not actually used. Only liburdfdom-headers-dev. --- rviz_marker_tools/CMakeLists.txt | 17 ++++++++++++----- .../rviz_marker_tools/marker_creation.h | 5 +++-- rviz_marker_tools/package.xml | 19 +++++++++++++++---- 3 files changed, 30 insertions(+), 11 deletions(-) diff --git a/rviz_marker_tools/CMakeLists.txt b/rviz_marker_tools/CMakeLists.txt index 08f6be61..ed931f8e 100644 --- a/rviz_marker_tools/CMakeLists.txt +++ b/rviz_marker_tools/CMakeLists.txt @@ -3,11 +3,14 @@ project(rviz_marker_tools) find_package(catkin REQUIRED COMPONENTS geometry_msgs - visualization_msgs roscpp + std_msgs tf2_eigen + visualization_msgs ) -find_package(urdfdom REQUIRED) +find_package(Eigen3 REQUIRED) + +find_package(urdfdom_headers REQUIRED) catkin_package( LIBRARIES @@ -16,10 +19,10 @@ catkin_package( include CATKIN_DEPENDS geometry_msgs + std_msgs visualization_msgs - roscpp DEPENDS - urdfdom + EIGEN3 ) set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}) @@ -35,7 +38,11 @@ add_library(${PROJECT_NAME} target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) target_include_directories(${PROJECT_NAME} PUBLIC $) -target_include_directories(${PROJECT_NAME} SYSTEM PRIVATE ${catkin_INCLUDE_DIRS}) +target_include_directories(${PROJECT_NAME} SYSTEM PRIVATE + ${catkin_INCLUDE_DIRS} + ${urdfdom_headers_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/rviz_marker_tools/include/rviz_marker_tools/marker_creation.h b/rviz_marker_tools/include/rviz_marker_tools/marker_creation.h index f4e3e55d..222456bd 100644 --- a/rviz_marker_tools/include/rviz_marker_tools/marker_creation.h +++ b/rviz_marker_tools/include/rviz_marker_tools/marker_creation.h @@ -1,8 +1,9 @@ #pragma once -#include -#include #include +#include +#include +#include namespace urdf { class Geometry; diff --git a/rviz_marker_tools/package.xml b/rviz_marker_tools/package.xml index 65256374..dcb4e7a8 100644 --- a/rviz_marker_tools/package.xml +++ b/rviz_marker_tools/package.xml @@ -9,9 +9,20 @@ catkin - visualization_msgs + eigen + eigen + geometry_msgs - roscpp - tf2_eigen - liburdfdom-dev + + liburdfdom-headers-dev + + roscpp + roscpp + + std_msgs + + tf2_eigen + tf2_eigen + + visualization_msgs From 99ccc115e041c5e80b8e653815a62936c3d392ec Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 17 Sep 2024 10:59:14 +0200 Subject: [PATCH 10/10] Update API: JumpThreshold -> CartesianPrecision (#611) Python: Access properties via writable references This allows to modify properties in place, e.g. cartesian_solver.precision.translational = 0.01 --- .../python/task_constructor/properties.h | 6 ++--- .../task_constructor/solvers/cartesian_path.h | 15 ++++++++---- core/python/bindings/src/solvers.cpp | 24 +++++++++++++++---- core/src/solvers/cartesian_path.cpp | 5 ++-- demo/src/fallbacks_move_to.cpp | 1 - 5 files changed, 35 insertions(+), 16 deletions(-) diff --git a/core/include/moveit/python/task_constructor/properties.h b/core/include/moveit/python/task_constructor/properties.h index 9e6a884c..d1119711 100644 --- a/core/include/moveit/python/task_constructor/properties.h +++ b/core/include/moveit/python/task_constructor/properties.h @@ -64,9 +64,9 @@ public: template class_& property(const char* name, const Extra&... extra) { PropertyConverter(); // register corresponding property converter - auto getter = [name](const type_& self) { - const moveit::task_constructor::PropertyMap& props = self.properties(); - return props.get(name); + auto getter = [name](type_& self) -> PropertyType& { + moveit::task_constructor::PropertyMap& props = self.properties(); + return const_cast(props.get(name)); }; auto setter = [name](type_& self, const PropertyType& value) { moveit::task_constructor::PropertyMap& props = self.properties(); diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 3595bc65..8de351da 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -39,6 +39,7 @@ #pragma once #include +#include namespace moveit { namespace task_constructor { @@ -57,13 +58,17 @@ public: void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); } void setStepSize(double step_size) { setProperty("step_size", step_size); } - void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); } + void setPrecision(const moveit::core::CartesianPrecision& precision) { setProperty("precision", precision); } + template + void setJumpThreshold(double) { + static_assert(std::is_integral::value, "setJumpThreshold() is deprecated. Replace with setPrecision."); + } void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); } [[deprecated("Replace with setMaxVelocityScalingFactor")]] // clang-format off - void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); } + void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); } // clang-format on [[deprecated("Replace with setMaxAccelerationScalingFactor")]] // clang-format off - void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); } + void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); } // clang-format on void init(const moveit::core::RobotModelConstPtr& robot_model) override; @@ -72,8 +77,8 @@ public: const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; }; } // namespace solvers diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index ef60fb82..572cf71e 100644 --- a/core/python/bindings/src/solvers.cpp +++ b/core/python/bindings/src/solvers.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include "utils.h" namespace py = pybind11; @@ -99,6 +100,22 @@ void export_solvers(py::module& m) { .property("max_step", "float: Limit any (single) joint change between two waypoints to this amount") .def(py::init<>()); + const moveit::core::CartesianPrecision default_precision; + py::class_(m, "CartesianPrecision", "precision for Cartesian interpolation") + .def(py::init([](double translational, double rotational, double max_resolution) { + return new moveit::core::CartesianPrecision{ translational, rotational, max_resolution }; + }), + py::arg("translational") = default_precision.translational, + py::arg("rotational") = default_precision.rotational, + py::arg("max_resolution") = default_precision.max_resolution) + .def_readwrite("translational", &moveit::core::CartesianPrecision::translational) + .def_readwrite("rotational", &moveit::core::CartesianPrecision::rotational) + .def_readwrite("max_resolution", &moveit::core::CartesianPrecision::max_resolution) + .def("__str__", [](const moveit::core::CartesianPrecision& self) { + return fmt::format("CartesianPrecision(translational={}, rotational={}, max_resolution={}", + self.translational, self.rotational, self.max_resolution); + }); + properties::class_(m, "CartesianPath", R"( Perform linear interpolation between Cartesian poses. Fails on collision along the interpolation path. There is no obstacle avoidance. :: @@ -108,15 +125,12 @@ void export_solvers(py::module& m) { # Instantiate Cartesian-space interpolation planner cartesianPlanner = core.CartesianPath() cartesianPlanner.step_size = 0.01 - cartesianPlanner.jump_threshold = 0.0 # effectively disable jump threshold. + cartesianPlanner.precision.translational = 0.001 )") .property("step_size", "float: Limit the Cartesian displacement between consecutive waypoints " "In contrast to joint-space interpolation, the Cartesian planner can also " "succeed when only a fraction of the linear path was feasible.") - .property( - "jump_threshold", - "float: Limit joint displacement between consecutive waypoints, thus preventing jumps in joint space. " - "This values specifies the fraction of mean acceptable joint motion per step.") + .property("precision", "Cartesian interpolation precision") .property("min_fraction", "float: Fraction of overall distance required to succeed.") .def(py::init<>()); diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index e2c0c80c..e2aadf66 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -54,7 +54,8 @@ CartesianPath::CartesianPath() { auto& p = properties(); p.declare("ik_frame", "frame to move linearly (use for joint-space target)"); p.declare("step_size", 0.01, "step size between consecutive waypoints"); - p.declare("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step"); + p.declare("precision", moveit::core::CartesianPrecision(), + "precision of linear path"); p.declare("min_fraction", 1.0, "fraction of motion required for success"); p.declare("kinematics_options", kinematics::KinematicsQueryOptions(), "KinematicsQueryOptions to pass to CartesianInterpolator"); @@ -112,7 +113,7 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath( &(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true, moveit::core::MaxEEFStep(props.get("step_size")), - moveit::core::JumpThreshold(props.get("jump_threshold")), is_valid, + props.get("precision"), is_valid, props.get("kinematics_options"), offset); assert(!trajectory.empty()); // there should be at least the start state diff --git a/demo/src/fallbacks_move_to.cpp b/demo/src/fallbacks_move_to.cpp index 25ab8738..05731ee1 100644 --- a/demo/src/fallbacks_move_to.cpp +++ b/demo/src/fallbacks_move_to.cpp @@ -35,7 +35,6 @@ int main(int argc, char** argv) { // setup solvers auto cartesian = std::make_shared(); - cartesian->setJumpThreshold(2.0); auto ptp = std::make_shared("pilz_industrial_motion_planner"); ptp->setPlannerId("PTP");