PILZ adjust cartesian interpolation #3330

MarcoMagriDev · 2025-02-08T09:52:26Z

Description

Added interpolation parameters
Sampled the trajectory using a uniform sampling_time computed from the equivalent rectangular motion profile (see this comment)
Adjusted sampling_time so that all points are equally spaced, including the last one
Modified pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow to no longer require sampling_time to be equal and uniform across the trajectories to be blended
add tests

this ensures that the new sample time will be lowered, thus not violating precomputed distance interpolation constraints at the same time avoids the issue on the last point generating small displacement interpolation

… trajectories from `pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow`

codecov-commenter · 2025-02-08T10:33:04Z

⚠️ Please install the to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

Attention: Patch coverage is 0% with 11 lines in your changes missing coverage. Please review.

Project coverage is 0.00%. Comparing base (9922704) to head (b25497e).
Report is 2 commits behind head on main.

Files with missing lines	Patch %	Lines
...strial_motion_planner/src/trajectory_functions.cpp	0.00%	11 Missing ⚠️

❗ Your organization needs to install the Codecov GitHub app to enable full functionality.

❗ There is a different number of reports uploaded between BASE (9922704) and HEAD (b25497e). Click for more details.

HEAD has 1 upload less than BASE

Flag BASE (9922704) HEAD (b25497e)

2 1

Additional details and impacted files

@@            Coverage Diff             @@
##             main   #3330       +/-   ##
==========================================
- Coverage   45.60%   0.00%   -45.59%     
==========================================
  Files         716     566      -150     
  Lines       62443   51174    -11269     
  Branches     7558    6832      -726     
==========================================
- Hits        28471       0    -28471     
- Misses      33805   51174    +17369     
+ Partials      167       0      -167

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

- computes shift of the second_trajectory using average instead of max. (this shift may be exactly the sample_time of the second trajectory if uniform) - reduces sampling_time that approximates continuous time to 0.001 increasing the minimum distance between points to 1mm - ensures the addition of the first and the last points Tests: - adapts `checkBlendingJointSpaceContinuity` and `checkBlendingCartSpaceContinuity` acceleration computations to take into account different time samples among the trajectories - fixes occurrences of `joint_velocity_tolerance` and `joint_accleration_tolerance` in `checkBlendingJointSpaceContinuity` - adjust `checkBlendingCartSpaceContinuity` to return False in case of failed checks

moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp

moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp

rr-mark

The approach here all looks sensible. I've left a few code review comments, and I'll try and find the time to look at those trajectory blender tests at some point this week.

MarcoMagriDev · 2025-02-10T08:44:34Z

@rr-mark thanks for your quick review

I will answer to each review separately.

Note that I am still working (on another branch) trying to develop the non constant sampling time on the LIN trajectory. See Problem 2.
The solution is not to change how velocity and acceleration accelerations are computed instead sampling the path at non constant sample time in order to ensure that points are distant enought for the numerical IK solver.
I am also adding some new tests for this PR

EDIT: here you can find a PR that tries to address the above mentioned problem. As soon as we agree to this version I can integrate those changes here or in another PR to start the discussion

rr-mark · 2025-02-11T10:02:50Z

moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp

+      idx_after = i + 1;
+      break;
+    }
+  }


Since getWayPointDurationFromStart(i) should monotonically increase with i, it should only be necessary to find the first value of i for which getWayPointDurationFromStart(i) > time.

For performance, it might also be worth passing in optional start and end indices to interpolatePose, so if we expect to call it multiple times in order we don't need to search the whole trajectory every time.

I just pushed a new version of this method incorporating your suggestions.

Let me know if you spot any issues or areas for improvement!

rr-mark

Since this PR is large, it might also be worth splitting it up - starting with a PR for just the trajectory blender.

rr-mark · 2025-02-11T11:19:49Z

moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp

+  // SLERP interpolation for rotation
+  Eigen::Quaterniond quat1(pose1.rotation());
+  Eigen::Quaterniond quat2(pose2.rotation());
+  interpolated_pose.linear() = quat1.slerp(lambda, quat2).toRotationMatrix();


It might be worth pulling out this code into a function to calculate interpolated_pose = interpolate(pose1, pose2, lambda). Looks like this function would also be useful in cartesian_interpolator.cpp.

(This function might also already exist as part of KDL).

You are right, thanks. I'll check if KDL already provides a function for this. If not, I'll implement a utility method to handle it.

Unfortunately, KDL doesn’t seem to provide a built-in way to achieve this.

Looking at cartesian_interpolator.cpp, I found that a common method could be useful here. However, I’m not sure if merging the two would be the best approach.

rr-mark · 2025-02-11T11:21:31Z

moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp

-  for (std::size_t i = 0; i < blend_sample_num; ++i)
+
+  // Define an arbitrary small sample time to sample the blending trajectory
+  double sampling_time = 0.001;


How easy would it be to use your nice interpolation parameters here instead of this arbitrary sampling time?

I like this approach since it also can helps ensure a minimum Cartesian distance similarly to what has been done here (which can be beneficial for numerical IK solvers).

My only concern is that adopting this would make splitting up the PR no longer a valid option.

Yeah, fair. I guess splitting the PR is nice to have but not necessary.

rr-mark · 2025-02-11T11:44:01Z

moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp

@@ -258,9 +319,21 @@ void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blendTra
    Eigen::Quaterniond end_quat(blend_sample_pose2.rotation());
    blend_sample_pose.linear() = start_quat.slerp(alpha, end_quat).toRotationMatrix();


(The interpolate function above would also be useful here: blend_sample_pose = interpolate(blend_sample_pose1, blend_sample_pose2, alpha).

yep, we only need to decide which is the best place for that utility method

For now, I’ve placed the method in trajectory_functions.

Let’s wait for input from a maintainer to see if they prefer making this method accessible from another dependency package, such as moveit_core, so it can be used here.

…interpolatePose` efficiency, minor refactor changes

…adds `checkInterpolationParameters` testutils, adds `interpolationParameters` unit test for LIN generator

MarcoMagriDev and others added 6 commits February 5, 2025 08:43

feat: adds first draft of interpolation_parameters

fcff676

Merge branch 'moveit:main' into feat/pilz-interpolation-params

f7003b0

fix: interpolation.min_number_of_samples correct type to int

3511d7c

fix: removes last trajectory segment with different sampling time.

eb933e5

this ensures that the new sample time will be lowered, thus not violating precomputed distance interpolation constraints at the same time avoids the issue on the last point generating small displacement interpolation

Merge branch 'moveit:main' into feat/pilz-interpolation-params

dbf5e8a

feat: remove requirements on const and equal sampling_time on blended…

8271d56

… trajectories from `pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow`

MarcoMagriDev mentioned this pull request Feb 8, 2025

PILZ LIN samples in time instead of space #3292

Open

MarcoMagriDev added 2 commits February 9, 2025 11:39

fix: clang-tidy, removes unnecessary time shift of the second trajectory

fefd9e1

rr-mark reviewed Feb 10, 2025

View reviewed changes

moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp Outdated Show resolved Hide resolved

rr-mark reviewed Feb 10, 2025

View reviewed changes

moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp Show resolved Hide resolved

rr-mark reviewed Feb 10, 2025

View reviewed changes

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp Outdated Show resolved Hide resolved

rr-mark reviewed Feb 10, 2025

View reviewed changes

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp Outdated Show resolved Hide resolved

rr-mark reviewed Feb 10, 2025

View reviewed changes

rr-mark reviewed Feb 11, 2025

View reviewed changes

MarcoMagriDev and others added 4 commits February 12, 2025 07:44

feat: adds interpolate function to trajectory_functions, improves `…

ea65de6

…interpolatePose` efficiency, minor refactor changes

fix: removes unused min_number_of_samples interpolation parameter, …

9479007

…adds `checkInterpolationParameters` testutils, adds `interpolationParameters` unit test for LIN generator

fix: clang-tidy

b25497e

Merge branch 'main' into feat/pilz-interpolation-params

0f54fda

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

PILZ adjust cartesian interpolation #3330

PILZ adjust cartesian interpolation #3330

MarcoMagriDev commented Feb 8, 2025 •

edited

Loading

codecov-commenter commented Feb 8, 2025 •

edited

Loading

rr-mark left a comment

MarcoMagriDev commented Feb 10, 2025 •

edited

Loading

rr-mark Feb 11, 2025

MarcoMagriDev Feb 12, 2025

rr-mark left a comment

rr-mark Feb 11, 2025

MarcoMagriDev Feb 11, 2025

MarcoMagriDev Feb 11, 2025

rr-mark Feb 11, 2025 •

edited

Loading

MarcoMagriDev Feb 11, 2025

rr-mark Feb 11, 2025

rr-mark Feb 11, 2025

MarcoMagriDev Feb 11, 2025

MarcoMagriDev Feb 12, 2025

		@@ -258,9 +319,21 @@ void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blendTra
		Eigen::Quaterniond end_quat(blend_sample_pose2.rotation());
		blend_sample_pose.linear() = start_quat.slerp(alpha, end_quat).toRotationMatrix();

PILZ adjust cartesian interpolation #3330

Are you sure you want to change the base?

PILZ adjust cartesian interpolation #3330

Conversation

MarcoMagriDev commented Feb 8, 2025 • edited Loading

Description

codecov-commenter commented Feb 8, 2025 • edited Loading

Codecov Report

rr-mark left a comment

Choose a reason for hiding this comment

MarcoMagriDev commented Feb 10, 2025 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rr-mark left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rr-mark Feb 11, 2025 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

MarcoMagriDev commented Feb 8, 2025 •

edited

Loading

codecov-commenter commented Feb 8, 2025 •

edited

Loading

MarcoMagriDev commented Feb 10, 2025 •

edited

Loading

rr-mark Feb 11, 2025 •

edited

Loading