Feature: New FREE command in Pilz planner for free-space trajectory generation

[AimanHaidar]

Description

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

Added Classes

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

• TrajectoryGeneratorFree
  Main class responsible for receiving the MotionPlanRequest and setting up the free-space path.

• PathFreeGenerator
  Handles the generation of free paths using KDL::Path_RoundedComposite.
  This class provides:

  • freeFromWaypoints() – 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 setFreePath()).

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 FREE command with MotionSequenceItem:

moveit_msgs::msg::MotionSequenceItem sequence_item;
sequence_item.req.planner_id = "FREE"
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.

[EzraBrooks]

Sorry, it's been a while since I've looked at the moveit2 codebase; do we have fmt available here? I think we should prefer modern string formatting techniques over ostringstream if so.

[AimanHaidar]

oh, this is a fix I have introduced it in #3604.
it is not related to this PR!

[AimanHaidar]

so I should replace this with fmt (modern formatting library)?
should I do it here or in the PR #3604 because I did a mistake by include this fix here.

[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
Also, One of the things encouraged me to work on this is when I wanted to use mtc with Sequence Blending I didn't find a way to use them together!
but with this maybe I will able to do somthing like:

auto pilz_free_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_, "pilz_industrial_motion_planner");
  pilz_free_planner->setPlannerId("FREE");
// .
// .
// .
auto stage = std::make_unique<mtc::stages::MoveTo>("weld", pilz_free_planner);
// set your waypoints as path constraints
task.add(std::move(stage))

[EzraBrooks]

I'll defer to the people who actually know something about motion planning now that they've chimed in.

[AimanHaidar]

To determine whether this new feature is worthwhile or redundant with Sequence of multiple segments, we have three cases:
1- "FREE" is a replacement for Sequence of multiple segments
2- "FREE" is redundant with Sequence of multiple segments
3- Both are good to work together

Assume we have a cut of hyperbolic curve with two branches and I want to move the tool through these two branches so I should do three steps:
first step : move the tool along the left branch
second step : move the tool from the end of the left branch to the start of the right branch
third step : move the tool along the right branch

In the first case, I definitely cannot perform the three steps with a single “FREE” path because it will be difficult to determine all waypoints and I wouldn’t be able to pause briefly between the steps.

In the second case, using a Sequence of multiple segments requires sending many items with the LIN command ID for the hyperbolic curves, which results in a large number of unnecessary joint-trajectory points (Comment)

In the third case, all I need is just a sequence of three items—("FREE","PTP","FREE")—and all done.

last thing: why this should be a seperate command or motion type and not consider it as LIN:
1- LIN refer to "Line" but this motion type surly not a line it is composite of lines and circles.
2- ... nothing here 😄 !

I hope this clarify everything.

[MatthijsBurgh]

I don't think we should add new .h header files.

[AimanHaidar]

ok, I will delete them

[AndyZe]

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.

Did the trajectories take the same time to execute, though? Extra waypoints doesn't bother me too much, but if the execution time is slower, that might be a good reason to replace the old version.

[AimanHaidar]

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.

Did the trajectories take the same time to execute, though? Extra waypoints doesn't bother me too much, but if the execution time is slower, that might be a good reason to replace the old version.

@AndyZe
I checked the the trajectory exeuation with combining LIN commands but I noticed the motion speed not change as expected just tiny changes and for most of the speeds it is so slow range between 26.9 to 36.5 seconds
but with "FREE" command all go right I double the speed and the execution time is halved and the motion took less than 4 seconds compared to the 26.9 seconds!

I'm not sure if the following is correct but assume I set the ros2 control update_rate to 50Hz. in the case of sequenced LIN command I got 408 joint trajectory points and my motion need 4 seconds to be executed so the controller need to send 400/4=100 point per second on average and this is more than the update rate of the ros2 control.

[AndyZe]

Ok, I'm convinced it's worth merging. Still needs to be named something other than "free". Sorry I don't have time for a detailed review.

[sea-bass]

What about PATH?

[AimanHaidar]

What about POLYLINE?

current suggestions:
blend
sequence
path
polyline