Feature: New POLYLINE command in Pilz planner for space trajectory generation

[AimanHaidar]

Description

This pull request adds a new Pilz Industrial Planner command: POLYLINE, which enables planning Cartesian polyline-space trajectories using waypoints.

Added Classes

The workflow of these classes follows the same structure as the existing CIRC and LIN commands.

• TrajectoryGeneratorPolyline
  Main class responsible for receiving the MotionPlanRequest and setting up the polyline-space path.

• PathPolylineGenerator
  Handles the generation of polyline paths using KDL::Path_RoundedComposite.
  This class provides:

  • polylineFromWaypoints() – Adds and validates waypoints.
  • computeBlendRadius() – Computes the maximum feasible rounding radius, scaled by the smoothness factor.
  • checkConsecutiveColinearWaypoints() – Detects and throws an error if three consecutive colinear points are found (handled in setPolylinePath()).

Notes:

• This feature requires the smoothness_level field in MotionPlanRequest (introduced in PR #190).
• it include what was pulled at Feature/make use of max cartesian speed in MotionPlanRequest with pilz planner #3608

---

Limitations

• The planner cannot generate trajectories that contain three or more consecutive colinear waypoints.
  This is a limitation of KDL::Path_RoundedComposite.
  When such a case is detected, an error is thrown and the plan is rejected.

---

Usage Example

An example of how to use the POLYLINE command with MotionSequenceItem:

moveit_msgs::msg::MotionSequenceItem sequence_item;
sequence_item.req.planner_id = "POLYLINE"
std::vector<geometry_msgs::msg::Pose> waypoints = // ...

geometry_msgs::msg::PoseStamped msg;
msg.header.frame_id = reference_frame;

for (const auto& waypoint : waypoints)
{
  msg.pose = waypoint;
  moveit_msgs::msg::PositionConstraint pos_constraint;
  pos_constraint.header.frame_id = reference_frame;
  pos_constraint.link_name = commanded_link;
  pos_constraint.constraint_region.primitive_poses.resize(1);
  pos_constraint.constraint_region.primitive_poses[0] = waypoint;
  pos_constraint.weight = 1.0;
  sequence_item.req.path_constraints.position_constraints.push_back(pos_constraint);
}

// Add the final point as the goal constraint
msg.pose = waypoints.back();
sequence_item.req.goal_constraints.push_back(
    kinematic_constraints::constructGoalConstraints(link_name, msg));

if (zero_blend_at_end)
  sequence_item.blend_radius = 0.0;  // No blending for the last waypoint

// Send the Sequence Item as in the MoveIt tutorials

Tests

triangles with different smoothness:

points generated from external parametric curve:

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[AimanHaidar]

@v4hn
@EzraBrooks
@nbbrooks

May you review this ?

[EzraBrooks]

Just one comment otherwise LGTM.

[AndyZe]

I'm not sure "free" is the right title. Suggesting some other names:

• variable blend --> variableBlendFromWaypoints()
• motion sequence --> sequenceFromWaypoints(). This is my favorite.

A free space planner typically means something like A* that is free to take any route to the goal.

[AndyZe]

Your triangle example seems to violate this? Did something change:

The planner cannot generate trajectories that contain three or more consecutive colinear waypoints (e.g., straight lines)

[AimanHaidar]

Your triangle example seems to violate this? Did something change:

The planner cannot generate trajectories that contain three or more consecutive colinear waypoints (e.g., straight lines)

the triangle example uses only triangle's vertices. what I meant there by "straight lines" is pure straight line.
if you give three points on the same line or give two points they make collinearity with the start point.
the restrictions come from
lines 96 to 102

[AimanHaidar]

I'm not sure "free" is the right title. Suggesting some other names:

• variable blend --> variableBlendFromWaypoints()
• motion sequence --> sequenceFromWaypoints(). This is my favorite.

A free space planner typically means something like A* that is free to take any route to the goal.

Right, FREE is not appropriate.
naming is a hard part!

[sea-bass]

I'm not active on here these days, but I saw this pop up on my feed. I wanted to point out that this feature seems to do the same thing as https://moveit.picknik.ai/main/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.html#sequence-of-multiple-segments -- so I would recommend 2 things for the author and reviewers:

1. Consider whether this new feature is either redundant with the above, or whether it's a better option that could allow removing the above. For example, if the existing blending feature actually allows blending different motion types (PTP, LIN, and CIRC), this PR may not be a full replacement. But I would check if this is the case.
2. If the decision is to keep this around, instead of making this be a new motion type, I'd keep it as LIN motion, but now if you specify intermediate waypoints as part of the message it automatically does the blending. After all, this is a blend of LIN motion segments.

Hope this helps more so than adds noise!

[AimanHaidar]

I'm not active on here these days, but I saw this pop up on my feed. I wanted to point out that this feature seems to do the same thing as https://moveit.picknik.ai/main/doc/how_to_guides/pilz_industrial_motion_planner/pilz_industrial_motion_planner.html#sequence-of-multiple-segments -- so I would recommend 2 things for the author and reviewers:

1. Consider whether this new feature is either redundant with the above, or whether it's a better option that could allow removing the above. For example, if the existing blending feature actually allows blending different motion types (PTP, LIN, and CIRC), this PR may not be a full replacement. But I would check if this is the case.

2. If the decision is to keep this around, instead of making this be a new motion type, I'd keep it as `LIN` motion, but now if you specify intermediate waypoints as part of the message it automatically does the blending. After all, this is a blend of LIN motion segments.

Hope this helps more so than adds noise!

@sea-bass
Thanks, This is what I have tried before. I used LIN command to generate general curves so I divided the curve into small segments but LIN seems to sample these small segments into many points as long lines, which ends up producing a lot of unnecessary points.
I noticed this when I visualized the path in RViz: using the LIN and Sequence method produced very dense points.

I tested both methods using points sampled from the ellipse:

$$ x = 0.1 \cos t,\qquad y = 0.05 \sin t,\qquad z = 0.0 $$

with $t \in [0,\ 2.0],\quad dt = 0.01$

the number of points in the planned joint_trajectory with the same max_cartesian_speed. I got:

# This is with LIN commands and Sequence
[INFO] [1763959333.823367295] [cartesian_planning]: TrajPoints 408
# This is with the new Command 
[INFO] [1763960677.326358520] [commands_loader]: TrajPoints : 29

But Why 200 ellipse waypoints to 29 joint trajectory points???

note: the number of joint trajectory points increase with decrease max_cartesian_speed

[AimanHaidar]

@sea-bass
@MatthijsBurgh
@AndyZe
@EzraBrooks

I have changed the name to POLYLINE
I think PATH is more general, but POLYLINE is the best one describes creating path of connecting many points with lines but the difference is there is arcs in between to smooth the path or make its derivative continuous.

If possible, I would like to rename it to SPOLYLINE, where the ‘S’ indicates smooth.

[codecov]

Codecov Report

❌ Patch coverage is 29.59184% with 138 lines in your changes missing coverage. Please review.
✅ Project coverage is 46.12%. Comparing base (760277f) to head (7ff2294).
⚠️ Report is 1 commits behind head on main.

Files with missing lines	Patch %	Lines
...tion_planner/src/trajectory_generator_polyline.cpp	5.27%	72 Missing ⚠️
...ial_motion_planner/src/path_polyline_generator.cpp	0.00%	49 Missing ⚠️
...n_planner/src/planning_context_loader_polyline.cpp	53.34%	7 Missing ⚠️
...ustrial_motion_planner/path_polyline_generator.hpp	0.00%	6 Missing ⚠️
...l_motion_planner/trajectory_generator_polyline.hpp	20.00%	4 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3610      +/-   ##
==========================================
- Coverage   46.19%   46.12%   -0.06%     
==========================================
  Files         720      726       +6     
  Lines       59181    59370     +189     
  Branches     7594     7614      +20     
==========================================
+ Hits        27333    27381      +48     
- Misses      31681    31822     +141     
  Partials      167      167              

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[EzraBrooks]

Looks like you're failing CI - it would also be nice to get some code coverage here. a 0% covered diff makes me anxious!

[AimanHaidar]

Looks like you're failing CI - it would also be nice to get some code coverage here. a 0% covered diff makes me anxious!

The failure is due to smoothness_level . see the first note in the PR description

about code coverage (codecov) I really don't understand what it is!!
I will learn what this then reply.

[AimanHaidar]

Mr @EzraBrooks I have added two tests for max_cartesian_speed in 0120591

also, remove unrelated fix 👇 introduced in #3604

[AimanHaidar]

after I updated the branch, the CI / hamble failed due to something not related to pilz ?!

[AimanHaidar]

@EzraBrooks
Now, all required CI checks have passed

[EzraBrooks]

what about @MatthijsBurgh's comment regarding .h files?

[AimanHaidar]

@EzraBrooks
It is the time to open PR for moveit2_tutorial.