Allow max Cartesian link speed in PlannerInterface (#277)
Some checks are pending
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci-testing TARGET_CMAKE_… (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.8 -e LSAN_OPTI… (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Waiting to run
CI / doc (push) Blocked by required conditions
CI / deploy (push) Blocked by required conditions
Format / pre-commit (push) Waiting to run

This commit is contained in:
Gauthier Hentz 2025-09-10 14:30:26 +02:00 committed by GitHub
parent ac4b92fff9
commit 7384702448
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
6 changed files with 25 additions and 1 deletions

View File

@ -89,6 +89,10 @@ public:
void setTimeout(double timeout) { properties_.set("timeout", timeout); } void setTimeout(double timeout) { properties_.set("timeout", timeout); }
void setMaxVelocityScalingFactor(double factor) { properties_.set("max_velocity_scaling_factor", factor); } void setMaxVelocityScalingFactor(double factor) { properties_.set("max_velocity_scaling_factor", factor); }
void setMaxAccelerationScalingFactor(double factor) { properties_.set("max_acceleration_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) { void setTimeParameterization(const trajectory_processing::TimeParameterizationPtr& tp) {
properties_.set("time_parameterization", tp); properties_.set("time_parameterization", tp);
} }

View File

@ -54,6 +54,8 @@ void export_solvers(py::module& m) {
.property<double>("max_velocity_scaling_factor", "float: Reduce the maximum velocity by scaling between (0,1]") .property<double>("max_velocity_scaling_factor", "float: Reduce the maximum velocity by scaling between (0,1]")
.property<double>("max_acceleration_scaling_factor", .property<double>("max_acceleration_scaling_factor",
"float: Reduce the maximum acceleration by scaling between (0,1]") "float: Reduce the maximum acceleration by scaling between (0,1]")
.property<double>("max_cartesian_speed", "float: maximum cartesian speed")
.property<std::string>("cartesian_speed_limited_link", "str: link with limited cartesian speed")
.def_property_readonly("properties", py::overload_cast<>(&PlannerInterface::properties), .def_property_readonly("properties", py::overload_cast<>(&PlannerInterface::properties),
py::return_value_policy::reference_internal, "Properties of the planner"); py::return_value_policy::reference_internal, "Properties of the planner");

View File

@ -40,6 +40,7 @@
#include <moveit/task_constructor/utils.h> #include <moveit/task_constructor/utils.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/time_parameterization.h> #include <moveit/trajectory_processing/time_parameterization.h>
#include <moveit/trajectory_processing/limit_cartesian_speed.h>
#include <moveit/kinematics_base/kinematics_base.h> #include <moveit/kinematics_base/kinematics_base.h>
#include <moveit/robot_state/cartesian_interpolator.h> #include <moveit/robot_state/cartesian_interpolator.h>
#include <tf2_eigen/tf2_eigen.h> #include <tf2_eigen/tf2_eigen.h>
@ -121,8 +122,19 @@ PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningScene
for (const auto& waypoint : trajectory) for (const auto& waypoint : trajectory)
result->addSuffixWayPoint(waypoint, 0.0); result->addSuffixWayPoint(waypoint, 0.0);
double max_cartesian_speed = props.get<double>("max_cartesian_speed");
auto timing = props.get<TimeParameterizationPtr>("time_parameterization"); auto timing = props.get<TimeParameterizationPtr>("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<std::string>("cartesian_speed_limited_link"))) {
ROS_INFO_STREAM("successfully set max " << max_cartesian_speed << " [m/s] for link "
<< props.get<std::string>("cartesian_speed_limited_link"));
} else {
ROS_ERROR_STREAM("failed to set max speed for link_ "
<< props.get<std::string>("cartesian_speed_limited_link"));
}
} else if (timing)
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"), timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor")); props.get<double>("max_acceleration_scaling_factor"));

View File

@ -149,6 +149,8 @@ void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMa
req.num_planning_attempts = p.get<uint>("num_planning_attempts"); req.num_planning_attempts = p.get<uint>("num_planning_attempts");
req.max_velocity_scaling_factor = p.get<double>("max_velocity_scaling_factor"); req.max_velocity_scaling_factor = p.get<double>("max_velocity_scaling_factor");
req.max_acceleration_scaling_factor = p.get<double>("max_acceleration_scaling_factor"); req.max_acceleration_scaling_factor = p.get<double>("max_acceleration_scaling_factor");
req.cartesian_speed_limited_link = p.get<std::string>("cartesian_speed_limited_link");
req.max_cartesian_speed = p.get<double>("max_cartesian_speed");
req.workspace_parameters = p.get<moveit_msgs::WorkspaceParameters>("workspace_parameters"); req.workspace_parameters = p.get<moveit_msgs::WorkspaceParameters>("workspace_parameters");
} }

View File

@ -50,6 +50,8 @@ PlannerInterface::PlannerInterface() {
p.declare<double>("timeout", std::numeric_limits<double>::infinity(), "timeout for planner (s)"); p.declare<double>("timeout", std::numeric_limits<double>::infinity(), "timeout for planner (s)");
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor"); p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor"); p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
p.declare<double>("max_cartesian_speed", 0.0, "maximum cartesian speed");
p.declare<std::string>("cartesian_speed_limited_link", "", "link with limited cartesian speed");
p.declare<TimeParameterizationPtr>("time_parameterization", std::make_shared<TimeOptimalTrajectoryGeneration>()); p.declare<TimeParameterizationPtr>("time_parameterization", std::make_shared<TimeOptimalTrajectoryGeneration>());
} }
} // namespace solvers } // namespace solvers

View File

@ -18,6 +18,8 @@ group = "panda_arm"
# [cartesianTut2] # [cartesianTut2]
# Cartesian and joint-space interpolation planners # Cartesian and joint-space interpolation planners
cartesian = core.CartesianPath() cartesian = core.CartesianPath()
cartesian.max_cartesian_speed = 0.1 # m/s
cartesian.cartesian_speed_limited_link = "panda_hand"
jointspace = core.JointInterpolationPlanner() jointspace = core.JointInterpolationPlanner()
# [cartesianTut2] # [cartesianTut2]