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!