From 7384702448650781111bfcd2f7878d91bb430024 Mon Sep 17 00:00:00 2001 From: Gauthier Hentz <64833674+gautz@users.noreply.github.com> Date: Wed, 10 Sep 2025 14:30:26 +0200 Subject: [PATCH 1/3] Allow max Cartesian link speed in PlannerInterface (#277) --- .../task_constructor/solvers/planner_interface.h | 4 ++++ core/python/bindings/src/solvers.cpp | 2 ++ core/src/solvers/cartesian_path.cpp | 14 +++++++++++++- core/src/solvers/pipeline_planner.cpp | 2 ++ core/src/solvers/planner_interface.cpp | 2 ++ demo/scripts/cartesian.py | 2 ++ 6 files changed, 25 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index ddc5a2ab..970583c8 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -89,6 +89,10 @@ public: void setTimeout(double timeout) { properties_.set("timeout", timeout); } void setMaxVelocityScalingFactor(double factor) { properties_.set("max_velocity_scaling_factor", factor); } void setMaxAccelerationScalingFactor(double factor) { properties_.set("max_acceleration_scaling_factor", factor); } + + void setMaxCartesianSpeed(double max) { setProperty("max_cartesian_speed", max); } + void setCartesianSpeedLimitedLink(const std::string& link) { setProperty("cartesian_speed_limited_link", link); } + void setTimeParameterization(const trajectory_processing::TimeParameterizationPtr& tp) { properties_.set("time_parameterization", tp); } diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index 26ac6c09..f4017922 100644 --- a/core/python/bindings/src/solvers.cpp +++ b/core/python/bindings/src/solvers.cpp @@ -54,6 +54,8 @@ void export_solvers(py::module& m) { .property("max_velocity_scaling_factor", "float: Reduce the maximum velocity by scaling between (0,1]") .property("max_acceleration_scaling_factor", "float: Reduce the maximum acceleration by scaling between (0,1]") + .property("max_cartesian_speed", "float: maximum cartesian speed") + .property("cartesian_speed_limited_link", "str: link with limited cartesian speed") .def_property_readonly("properties", py::overload_cast<>(&PlannerInterface::properties), py::return_value_policy::reference_internal, "Properties of the planner"); diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index c87bbef1..0d604fab 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -121,8 +122,19 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene for (const auto& waypoint : trajectory) result->addSuffixWayPoint(waypoint, 0.0); + double max_cartesian_speed = props.get("max_cartesian_speed"); auto timing = props.get("time_parameterization"); - if (timing) + // compute timing to move the eef with constant speed + if (max_cartesian_speed > 0.0) { + if (trajectory_processing::limitMaxCartesianLinkSpeed(*result, max_cartesian_speed, + props.get("cartesian_speed_limited_link"))) { + ROS_INFO_STREAM("successfully set max " << max_cartesian_speed << " [m/s] for link " + << props.get("cartesian_speed_limited_link")); + } else { + ROS_ERROR_STREAM("failed to set max speed for link_ " + << props.get("cartesian_speed_limited_link")); + } + } else if (timing) timing->computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), props.get("max_acceleration_scaling_factor")); diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 99c5912c..34e71a07 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -149,6 +149,8 @@ void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMa req.num_planning_attempts = p.get("num_planning_attempts"); req.max_velocity_scaling_factor = p.get("max_velocity_scaling_factor"); req.max_acceleration_scaling_factor = p.get("max_acceleration_scaling_factor"); + req.cartesian_speed_limited_link = p.get("cartesian_speed_limited_link"); + req.max_cartesian_speed = p.get("max_cartesian_speed"); req.workspace_parameters = p.get("workspace_parameters"); } diff --git a/core/src/solvers/planner_interface.cpp b/core/src/solvers/planner_interface.cpp index a58b110d..8475699d 100644 --- a/core/src/solvers/planner_interface.cpp +++ b/core/src/solvers/planner_interface.cpp @@ -50,6 +50,8 @@ PlannerInterface::PlannerInterface() { p.declare("timeout", std::numeric_limits::infinity(), "timeout for planner (s)"); p.declare("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor"); p.declare("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor"); + p.declare("max_cartesian_speed", 0.0, "maximum cartesian speed"); + p.declare("cartesian_speed_limited_link", "", "link with limited cartesian speed"); p.declare("time_parameterization", std::make_shared()); } } // namespace solvers diff --git a/demo/scripts/cartesian.py b/demo/scripts/cartesian.py index a7936012..8047034a 100755 --- a/demo/scripts/cartesian.py +++ b/demo/scripts/cartesian.py @@ -18,6 +18,8 @@ group = "panda_arm" # [cartesianTut2] # Cartesian and joint-space interpolation planners cartesian = core.CartesianPath() +cartesian.max_cartesian_speed = 0.1 # m/s +cartesian.cartesian_speed_limited_link = "panda_hand" jointspace = core.JointInterpolationPlanner() # [cartesianTut2] From 1655762a6350d5c7e73feee624cc1fd1f7ce6509 Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Wed, 10 Sep 2025 11:33:31 -0400 Subject: [PATCH 2/3] Avoid duplicate scenes in Solution.msg from generator stages (#639) If start and end scene of a stage are identical (e.g. from a generator), we can use an (empty) scene diff as well. --- core/src/storage.cpp | 3 ++- core/test/CMakeLists.txt | 1 + core/test/test_storage.cpp | 35 +++++++++++++++++++++++++++++++++++ 3 files changed, 38 insertions(+), 1 deletion(-) create mode 100644 core/test/test_storage.cpp diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 99dda1e9..dc737902 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -231,7 +231,8 @@ void SubTrajectory::appendTo(moveit_task_constructor_msgs::Solution& msg, Intros if (trajectory()) trajectory()->getRobotTrajectoryMsg(t.trajectory); - if (this->end()->scene()->getParent() == this->start()->scene()) + if (this->end()->scene()->getParent() == this->start()->scene() || // diff + this->end()->scene() == this->start()->scene()) // identical (from generator) this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff); else this->end()->scene()->getPlanningSceneMsg(t.scene_diff); diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index c01023cb..de86e848 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -42,6 +42,7 @@ if (CATKIN_ENABLE_TESTING) mtc_add_gmock(test_pruning.cpp) mtc_add_gtest(test_properties.cpp) mtc_add_gtest(test_cost_terms.cpp) + mtc_add_gtest(test_storage.cpp) mtc_add_gmock(test_fallback.cpp) mtc_add_gmock(test_cost_queue.cpp) diff --git a/core/test/test_storage.cpp b/core/test/test_storage.cpp new file mode 100644 index 00000000..f6a33934 --- /dev/null +++ b/core/test/test_storage.cpp @@ -0,0 +1,35 @@ +#include "models.h" + +#include +#include + +#include + +#include +#include + +using namespace moveit::task_constructor; +using namespace planning_scene; +using namespace moveit::core; + +// https://github.com/moveit/moveit_task_constructor/issues/638 +TEST(SolutionMsg, DuplicateScenes) { + Task t; + PlanningScenePtr scene; + + t.setRobotModel(getModel()); + scene = std::make_shared(t.getRobotModel()); + t.add(std::make_unique("start", scene)); + + EXPECT_TRUE(t.plan(1)); + EXPECT_EQ(t.solutions().size(), 1u); + + // create solution + moveit_task_constructor_msgs::Solution solution_msg; + t.solutions().front()->toMsg(solution_msg); + + // all sub trajectories `scene_diff` should be a diff + EXPECT_EQ(solution_msg.sub_trajectory.size(), 1u); + EXPECT_EQ(solution_msg.start_scene.is_diff, false); + EXPECT_EQ(solution_msg.sub_trajectory.front().scene_diff.is_diff, true); +} From 377d09046f164a6b4f6b99b5e53a2f8f2ffe2954 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 9 Oct 2025 14:47:27 +0200 Subject: [PATCH 3/3] Mandatory yaml dependency --- .../properties/CMakeLists.txt | 11 ++++------- .../properties/property_factory.cpp | 17 ----------------- visualization/package.xml | 1 + 3 files changed, 5 insertions(+), 24 deletions(-) diff --git a/visualization/motion_planning_tasks/properties/CMakeLists.txt b/visualization/motion_planning_tasks/properties/CMakeLists.txt index f9fa7de0..edbb46e5 100644 --- a/visualization/motion_planning_tasks/properties/CMakeLists.txt +++ b/visualization/motion_planning_tasks/properties/CMakeLists.txt @@ -2,16 +2,13 @@ set(MOVEIT_LIB_NAME motion_planning_tasks_properties) set(SOURCES property_factory.cpp + property_from_yaml.cpp ) find_package(PkgConfig REQUIRED) -pkg_check_modules(YAML yaml-0.1) -if (YAML_FOUND) - # Only cmake > 3.12 provides XXX_LINK_LIBRARIES. Find the absolute path manually - find_library(YAML_LIBRARIES ${YAML_LIBRARIES} PATHS ${YAML_LIBRARY_DIRS}) - list(APPEND SOURCES property_from_yaml.cpp) - add_definitions(-DHAVE_YAML) -endif() +pkg_check_modules(YAML REQUIRED yaml-0.1) +# Only cmake > 3.12 provides XXX_LINK_LIBRARIES. Find the absolute path manually +find_library(YAML_LIBRARIES ${YAML_LIBRARIES} PATHS ${YAML_LIBRARY_DIRS}) add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCES}) diff --git a/visualization/motion_planning_tasks/properties/property_factory.cpp b/visualization/motion_planning_tasks/properties/property_factory.cpp index bc347391..f34f7cdd 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.cpp +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -147,21 +147,4 @@ void PropertyFactory::addRemainingProperties(rviz::Property* root, mtc::Property new rviz::Property("no properties", QVariant(), QString(), root); } -#ifndef HAVE_YAML -rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& /*type*/, - const std::string& description, const std::string& value, - rviz::Property* old) { - if (old) { // reuse existing Property? - assert(old->getNameStd() == name); - old->setDescription(QString::fromStdString(description)); - old->setValue(QString::fromStdString(value)); - return old; - } else { // create new Property? - rviz::Property* result = new rviz::StringProperty(QString::fromStdString(name), QString::fromStdString(value), - QString::fromStdString(description)); - result->setReadOnly(true); - return result; - } -} -#endif } // namespace moveit_rviz_plugin diff --git a/visualization/package.xml b/visualization/package.xml index 3dfb1468..9bbf1ff4 100644 --- a/visualization/package.xml +++ b/visualization/package.xml @@ -18,6 +18,7 @@ moveit_ros_visualization roscpp rviz + yaml rosunit rostest