PILZ LIN samples in time instead of space

[MarcoMagriDev]

Description

The LIN pipeline of pilz_industrial_motion_planner samples IK solutions along the Cartesian path using a fixed sampling time (0.1 sec). Once the IK solutions are obtained, a joint-space interpolation is performed.

Since the Cartesian path is sampled based on time rather than space, the linearity of the resulting path degrades as the commanded motion speed increases.

For example, consider a motion of 1 meter:

• If commanded at 1 m/s, 10 Cartesian samples will be extracted along the path.
• If commanded at 5 m/s, only 2 samples will be extracted.

Since these points are then interpolated in joint space, the first motion will appear more linear than the second due to the higher number of samples along the Cartesian path.

ROS Distro

Humble

OS and version

Ubuntu 22.04

Source or binary build?

Source

If binary, which release version?

No response

If source, which branch?

humble

Which RMW are you using?

CycloneDDS

Steps to Reproduce

Command the same LIN motion at different speed and analyze resulting path linearity

Expected behavior

The linearity of the generated path should remain independent of the commanded speed.

Actual behavior

The linearity of the generated path is dependent of the commanded speed.

Backtrace or Console output

No response

[sea-bass]

It really does that? Weird.

While I think Pilz could definitely be rewritten to be like 10% of the amount of the code it currently is, that would take a while -- especially because of the blending functionality.

My suggestion would be to find a way to elevate that 0.1 seconds hard-coded value to an input parameter, thereby exposing a way to parameterize IK solve resolution by distance.

At least for the LIN profile, that would be an easy calculation based on linear and angular distances. For the CIRC profile, I guess it's also doable since there are formulas for arc lengths based on descriptions of a circle.

[MarcoMagriDev]

It really does that? Weird.

Correct me if I am wrong but the value is hardcoded here and gets forwarded till here where IKs are performed for each time samples.

While I think Pilz could definitely be rewritten to be like 10% of the amount of the code it currently is, that would take a while -- especially because of the blending functionality.

Totally agree with you.

My suggestion would be to find a way to elevate that 0.1 seconds hard-coded value to an input parameter, thereby exposing a way to parameterize IK solve resolution by distance.

At least for the LIN profile, that would be an easy calculation based on linear and angular distances. For the CIRC profile, I guess it's also doable since there are formulas for arc lengths based on descriptions of a circle.

Even though having this value hardcoded isn’t ideal, simply exposing it as a parameter wouldn’t fully solve the problem. Instead, I think it would be better to introduce two parameters: max_translation_interpolation_distance and max_rotation_interpolation_distance. The planner could then compute the required sampling times dynamically to meet these interpolation constraints.

Correct me if I’m wrong, but assuming a rectangular velocity profile, the sampling time should remain constant along the path. However, with a trapezoidal profile, it wouldn’t be constant. That said, if we compute a fixed sampling time based on the rectangular profile assumption, I think that the constraints should still hold at the acceleration and deceleration phases of a trapezoidal profile.

[sea-bass]

Wow yes, that default value of 0.1 is not used at all at the top level of Pilz -- wild.

moveit2/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.hpp

Line 129 in 2302268

generator_.generate(getPlanningScene(), request_, res);

And that makes sense -- I'm reading the Pilz code now and it seems the general step is:

1. Generate Cartesian velocity profile with cartesianTrapVelocityProfile() (no sample time used)
2. Call generateJointTrajectory():
   a. This loops through the trajectory profile using the range [0 : sampling_time : trajectory.Duration()]

So yes, I guess you could do what you suggested by checking the KDL velocity profile's Pos() output to see what the translation and/or orientation offsets between sample times is.

If these are less than the max_<translation/rotation>_interpolation_distance parameters, you could just scale down the sample time. FOR EXAMPLE:

• For some sample time dt (hopefully no longer hard-coded to 0.1), compute all trajectory.Pos(t) and trajectory.Pos(t+dt), compute the position and orientation change.
• Find the maximum of each of these values. Let's say max position change is 0.2 m and max orientation change is 0.6 radians.
• Compare these against the maximum limits specified by the user. Let's say max position change is 0.1 m (1/2 of the actual) and max orientation change is 0.4 radians (2/3 of the actual).
• Given these limits, you can either
  • Simpler but less efficient: Just increase the entire sample time by 2, solve 2x the IK problems
  • A bit more complicated but more efficient: Only resample between two points where the max position and/or orientation is exceeded

Downstream of this, I have no idea how well the rest of Pilz handles non-uniform sampling times... so that may also help decide which of the 2 possible solutions to take.

EDIT: It seems there are places that explicitly check for uniform sampling time, like this function used in blending.

[MarcoMagriDev]

You're absolutely right—there are multiple places in PILZ where sample_times are assumed or checked to be constant. That said, I completely agree that the ideal solution would be to move away from a constant sampling time.

As a possible simplification, we could consider the following approach:

• Define max_translation_interpolation_distance = 0.005 and max_rotation_interpolation_distance = 0.1.
• Compute the total translation Dt and rotation Dr distances between the start and end of the trajectory.
• Determine the number of interpolation steps required:
  • It = Dt / max_translation_interpolation_distance
  • Ir = Dr / max_rotation_interpolation_distance
• Compute the maximum allowable velocity based on cartesian_limits and max_velocity_scaling_factor:
  • Vt for translation
  • Vr for rotation
• Determine the minimum time required to complete the motion:
  • Tt = Dt / Vt
  • Tr = Dr / Vr
• Compute sample_time as min(Tt / It, Tr / Ir). By sybstituting and simplifing min( max_translation_interpolation_distance / Vt, max_rotation_interpolation_distance / Vr) this means we can directly compute sample_time without needing any additional information about trajectory positions.

This approach ensures a constant sampling time based on a rectangular velocity profile assumption. If applied to a motion with a trapezoidal profile, it should guarantees that the sampled points along the trajectory will be closer than the specified interpolation distances, even if they are not perfectly equidistant in Cartesian space.

What do you think?

[sea-bass]

I think it's certainly an improvement from hard-coded 0.1 seconds!

It does actually make some points closer together and others farther apart, though... since a rectangular velocity profile with the same duration as a trapezoidal one necessarily has a lower max velocity. So I'm sort of worried about the "max distance" value not actually being honored vs. a pure sample time one. See for example the following image from this link

It could also be a "max sample time" + "min number of points" configuration. So for example, sample trajectory at 0.1 seconds / 20 points, meaning if you have a quicker trajectory it will upsample it to ensure there are 20 points. On the other hand, if you have a longer trajectory it might have more than 20 points, but will be sampled at 0.1 seconds.

We can start with your idea and see how it goes; I imagine it will touch more of the code and testing paths than we might imagine.

[MarcoMagriDev]

I think it's certainly an improvement from hard-coded 0.1 seconds!

That's for sure! 😄

It does actually make some points closer together and others farther apart, though... since a rectangular velocity profile with the same duration as a trapezoidal one necessarily has a lower max velocity. So I'm sort of worried about the "max distance" value not actually being honored vs. a pure sample time one. See for example the following image from this link

I am not requiring the two motion profiles to have the same duration, but rather the same maximum velocity. If I ensure that the sampling time in the max-velocity region is small enough such that the displacement in each sample is below the threshold, I am confident that during the acceleration and deceleration phases, the same sampling time will result in even smaller displacements (since the velocity is not at its peak). Does this address your concern about this approach?

It could also be a "max sample time" + "min number of points" configuration. So for example, sample trajectory at 0.1 seconds / 20 points, meaning if you have a quicker trajectory it will upsample it to ensure there are 20 points.

I really like this configuration option expecially for PTP motions.

We can start with your idea and see how it goes; I imagine it will touch more of the code and testing paths than we might imagine.

I'll create a branch to start working on this.

[sea-bass]

I am not requiring the two motion profiles to have the same duration, but rather the same maximum velocity. If I ensure that the sampling time in the max-velocity region is small enough such that the displacement in each sample is below the threshold, I am confident that during the acceleration and deceleration phases, the same sampling time will result in even smaller displacements (since the velocity is not at its peak). Does this address your concern about this approach?

I think I get it now -- if the max speed is that from the Cartesian limits, and not from matching the duration of the trapezoidal profile, this should indeed provide a guaranteed bound. Thanks!

[sea-bass]

And in your equations, my proposal would be trivial to add:

dt = min(Tt / It, Tr / Ir, Tmax)

Where Tmax is the max allowable sample time regardless of distance, which could be a parameter

[MarcoMagriDev]

And in your equations, my proposal would be trivial to add:

dt = min(Tt / It, Tr / Ir, Tmax)

Where Tmax is the max allowable sample time regardless of distance, which could be a parameter

What do you think about setting this Tmax equal to the current sampling_time, but exposing it as a parameter? This way, if the max_<translation/rotation>_interpolation_distance parameters are not provided (and default to inf), the behavior would remain the same as the current implementation.

---

Moreover, I want to point out that by implementing what we're planning, the planning time will drastically increase due to additional collision checking. For this reason, I would suggest adding two more parameters: min_<translation/rotation>_collision_checking_distance, which would specify the minimum distance between two samples in order to perform collision checks.

[sea-bass]

I would say yes to the sampling time, and "not yet" to the collision checking idea.

Let's first see how it goes with the ability to configure sample times/distances to trade off density of waypoints vs. time spent solving IK/collision checking those IK solutions.

Especially if you have ideas for how to make the defaults compatible with existing behavior, I'm in favor of building up more functionality/optimizations as they are further needed.

[Danilrivero]

Bumped into this error as well when planning with LIN at higher velocities, specially with rotations, appreciate this.

You're absolutely right—there are multiple places in PILZ where sample_times are assumed or checked to be constant. That said, I completely agree that the ideal solution would be to move away from a constant sampling time.

As a possible simplification, we could consider the following approach:

* Define `max_translation_interpolation_distance = 0.005` and `max_rotation_interpolation_distance = 0.1`.

* Compute the total translation `Dt` and rotation `Dr` distances between the start and end of the trajectory.

* Determine the number of interpolation steps required:
  
  * `It = Dt / max_translation_interpolation_distance`
  * `Ir = Dr / max_rotation_interpolation_distance`

* Compute the maximum allowable velocity based on `cartesian_limits` and `max_velocity_scaling_factor`:
  
  * `Vt` for translation
  * `Vr` for rotation

* Determine the minimum time required to complete the motion:
  
  * `Tt = Dt / Vt`
  * `Tr = Dr / Vr`

* Compute sample_time as `min(Tt / It, Tr / Ir)`. By sybstituting and simplifing `min( max_translation_interpolation_distance / Vt,  max_rotation_interpolation_distance / Vr)` this means we can directly compute sample_time without needing any additional information about trajectory positions.

This approach ensures a constant sampling time based on a rectangular velocity profile assumption. If applied to a motion with a trapezoidal profile, it should guarantees that the sampled points along the trajectory will be closer than the specified interpolation distances, even if they are not perfectly equidistant in Cartesian space.

What do you think?

Did you bump into any issues while planning with LIN with the joint_limits?

I had cases in which the generated planned path contained joint velocities above these limits throwing an error. Just a thought, could the joint_limits be respected in this case through this proposal in a way that said velocity profile will always be below the joint_limits even though you do not accomplish reaching the max. defined values at cartesian_limits?

There is a whole discussion about this very own issue here moveit/moveit#3553 as well by the way. I may have missed some conversations about this but it felt like I did not reach a full control of the velocity profiles I was expecting by having to verify both files have proper values, and even then, it could enter a scenario (due to the issue described here) in which it could fail.

While I think Pilz could definitely be rewritten to be like 10% of the amount of the code it currently is, that would take a while -- especially because of the blending functionality.

Looks like a good GSOC to me 😃

[MarcoMagriDev]

@Danilrivero Thank you for linking this interesting PR.

Did you bump into any issues while planning with LIN with the joint_limits?

During my preliminary tests, I encountered the joint_limits issue that these guys described.

Reducing the sample time amplifies the velocity and acceleration values, causing joint_limits checks to fail. This is even more critical during acceleration and deceleration phases (as the last trajectory point in the PR is) since, in these regions, a constant sample time results in the smallest possible Cartesian motions (in terms of distance).

Why are small motions problematic?

The issue with small motions is that they exacerbate the impact of the numerical inverse kinematics (IK) solver's threshold (e.g., KDL, Track-IK) on the computed derivative.

Let’s illustrate this with a numerical example:

• In the worst case, the effective distance between two consecutive trajectory points will be d + 2ε, leading to a Cartesian velocity norm of:

  $$ v = \frac{d + 2\epsilon}{t_s} $$

  Which can be rewritten as:

  $$ v = \frac{d}{t_s} + \frac{2\epsilon}{t_s} = v_{\text{nom}} + v_{\text{noise}} $$

• When d ≫ ε, the noise term v_noise is negligible compared to v_nom.

• However, when the samples are too close to each other, v_noise becomes significant, leading to violations of velocity limits.

I didn’t frame this in terms of joint-space velocity due to the nonlinear nature of the Cartesian-to-joint relationship, but this reasoning in Cartesian space helps explain why excessively close samples cause velocity limit violations.

---

Possible Problems and Solutions for the Proposed Approach

Problem 1:

The last point of the trajectory may not have the same sampling time as the others if the path length is not a multiple of the most conservative interpolation.

Proposed Solution:
Use an interpolation value that is a multiple of the path length but still below the specified limit. This ensures that points remain closer than the specified value while maintaining a consistent sampling time across the entire trajectory.

---

Problem 2:

As stated in previous comments, if we choose a constant sample time to be compatible with the blending functionality, the points at the beginning and end of the trajectory will have the smallest displacement (assuming equal acceleration and deceleration limits a_max):

$$ d = \frac{1}{2} \cdot a_{\text{scaling}} \cdot a_{\text{max}} \cdot t_s^2 $$

If this displacement is comparable to the inverse kinematics (IK) solver threshold, the motion may fail due to joint limit violations.

Proposed Solution:
Instead of using backward differences to compute joint velocities, use the Jacobian. However, still use backward differences for accelerations.

[Danilrivero]

Hi,

Really appreciate your insightful comment about the issues related to joint_limits .

Problem 1:
The last point of the trajectory may not have the same sampling time as the others if the path length is not a multiple of the most conservative interpolation.

Proposed Solution:
Use an interpolation value that is a multiple of the path length but still below the specified limit. This ensures that points remain closer than the specified value while maintaining a consistent sampling time across the entire trajectory.

I agree with all of this, seems to be the proper solution to the existent problem with accelerations encountered.

Problem 2:

Proposed Solution:
Instead of using backward differences to compute joint velocities, use the Jacobian. However, still use backward differences for accelerations.

Agree with you in everything other than proposing the use of the Hessian for the accelerations if you are going for it, don't know the possible downsides. Also taking into account the singularities probably.

Overall all your proposed ideas are great, it seems to be a considerable rework of the whole PILZ planner though, if you are going for it I am down to assist whenever possible.

[MarcoMagriDev]

I started working on this fix here
Unfortunately, this already touches many files, but I couldn't find a cleaner way to achieve the desired behavior differently.

Completed so far:

• Created a new parameter file (interpolation_parameters.yaml) that exposes the required parameters using generate_parameter_library.
• Implemented interpolation::ParamsListener in the planner and passed it to all planning contexts to update parameter values before calling TrajectoryGenerator::generate.
• Replaced sample_time with interpolation::Params in all trajectory generators.
• Adapted tests accordingly.

Remaining tasks:

• Handle the last trajectory point correctly.
• Adapt velocity computation to use Jacobians and update acceleration computation to use backward differences on velocity.

If anyone has time, a review would be greatly appreciated! Let me know if you see any possible improvements or concerns with the current approach.