b»robotized moveit plugins

When we add more plugins, add to this README.md

Moveit plugins developed by b»robotized.

cubic_spline_smoothing_adapter

MoveIt 2 Planning Response Adapter that recalculates waypoint velocities and accelerations using $C^2$ (cubic) splines to get smoother joint-space trajectories.

Mostly developed to post-process PILZ LIN planning!

Motivation

Motivated by jerky motion we observed when executing trajectories generated by PILZ LIN from Moveit2 and executed with JointTrajectoryController on industrial robots.

When cartesian path and trapezioidal velocity profile come out of MoveIt2 after planning, the points are converted to joint space using IK. This produces n points, each 0.1s distant, with not so nice velocity and acceleration to actually reach those points.

JointTrajectoryController struggles to interpolate between the waypoints, producing very oscillatory velocity and acceleration as it tries to fit its 5th order spline to desired waypoint values.

Those oscillations inform the final position sent, causing small "wobbles" in position commands, which the robot controller faithfully executes, shaking along the way.

Algorithm/Implementation

This plugin recalculates velocities and accelerations after conversion to joint-space by fitting cubic splines to position waypoints, so the joint-space trajectory is smooth. The pipeline is:

• Trajectory segment seams: The trajectory is split into discrete segments at zero-velocity "seams", for cases when we have mutliple start-stop in a single motion command. This isolates continuous portions of the motion profiles during, for example, pick-and-place tasks.
• Spline fitting: A cubic spline is fitted to each segment, clamping first-derivative boundary conditions to the requested start/end velocities.
• Analytical Peak Evaluation: The plugin finds the exact peak values for velocity, acceleration, and path deviation across the motion profiles.
• Time Dilation: Peak values are checked against MoveIt 2's VariableBounds. If hardware limits are violated, a time stretch factor $k$ is calculated required to not break them. This way we can say "just execute this, even if you have to do it slower, up to max_time_stretch % of the original trajectory.

Parameters

Parameter	Type	Default	Description
path_dev_max	double	0.02	Maximum allowed joint-space position deviation (in radians) from the original linear path between waypoints.
pos_tol	double	0.005	Tolerance margin applied when checking if smoothed peaks violate joint position bounds.
max_time_stretch	double	1.20	Maximum allowable time dilation factor (e.g., 1.20 = 20% stretch) before the adapter rejects the trajectory.
seam_eps	double	1e-6	Velocity threshold used to identify zero-velocity seams and split the trajectory into independent spline segments.

Quick Start

To activate the adapter, pull, build and append it to your MoveIt 2 planning pipeline configuration.

pilz:
    planning_plugins:
    - pilz_industrial_motion_planner/CommandPlanner
    default_planner_config: PTP
    request_adapters:
    - default_planner_request_adapters/ResolveConstraintFrames
    - default_planner_request_adapters/FixWorkspaceBounds
    - cubic_spline_smoothing_adapter/CubicSplineSmoothingAdapter
    path_dev_max: 0.05
    pos_tol: 0.01
    max_time_stretch: 1.50
    seam_eps: 1e-5