Investigation of Velocity and Acceleration Scaling Factors in Pilz Industrial Motion Planner

[lkaising]

Background

I'm investigating the relationship between velocity/acceleration scaling factors and their impact on trajectory planning in the Pilz Industrial Motion Planner, specifically when using the "LIN" motion type. The key functions in question are:

planner->setMaxVelocityScalingFactor(vel_scaling);
planner->setMaxAccelerationScalingFactor(acc_scaling);

Observed Behavior

I've encountered some unexpected behavior when working with these scaling factors in moveRelative tasks. Here are two test cases that demonstrate the issue:

Test Case 1: Scaling Factor 0.1

Created Task: 'moveRelative:0:0.005:0'
  [Stage 1] MOVE_RELATIVE: Delta( 0.0000,  0.0050,  0.0000)
                           Vel: 0.1000; Acc: 0.1000
[moveit.pilz_industrial_motion_planner.trajectory_functions]: Joint velocity limit of joint_2 violated. 
Set the velocity scaling factor lower! 
Actual joint velocity is 16.2388, while the limit is 8.11578.

Test Case 2: Scaling Factor 0.00011

Created Task: 'moveRelative:0:0.005:0:0.00011:0.00011'
  [Stage 1] MOVE_RELATIVE: Delta( 0.0000,  0.0050,  0.0000)
                           Vel: 0.0001; Acc: 0.0001
[moveit.pilz_industrial_motion_planner.trajectory_functions]: Joint velocity limit of joint_2 violated. 
Set the velocity scaling factor lower! 
Actual joint velocity is 16.2388, while the limit is 8.11578.

Questions and Observations

1. Despite significantly different scaling factors (0.1 vs 0.00011), both scenarios result in the same actual joint velocity (16.2388).
2. The joint velocity limit violation persists regardless of the scaling factor used.
3. The relationship between the scaling factors and the actual trajectory planning is unclear.

Investigation Attempts

I've looked into the implementation files for both:

• Pilz motion planner
• Planner interface

However, I've been unable to trace the direct connection between setting the scaling factors and their influence on the trajectory planning process.

Key Questions

1. Why does the actual joint velocity remain constant despite different scaling factors?
2. Are the scaling factors actually being applied to the trajectory plan?
3. What is the underlying mechanism that translates these scaling factors into the final trajectory?

Any insights into how these scaling factors are implemented and their expected behavior would be greatly appreciated.

[rhaschke]

MTC just passes the scaling factors to the PlanningRequest:

moveit_task_constructor/core/src/solvers/pipeline_planner.cpp

Lines 150 to 151 in 8318546

	req.max_velocity_scaling_factor = p.get<double>("max_velocity_scaling_factor");
	req.max_acceleration_scaling_factor = p.get<double>("max_acceleration_scaling_factor");

MoveIt's planning pipeline passes those values to the TimeParameterization plugin:

  virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
                                 const double max_velocity_scaling_factor = 1.0,
                                 const double max_acceleration_scaling_factor = 1.0) const = 0;

However, the Pilz planners do not use a TimeParameterization plugin, but compute the trajectory timing as part of motion generation.

Please verify whether the scaling factor is correctly conveyed to the Pilz planner, by inspecting the values here:

void TrajectoryGenerator::validateRequest(const planning_interface::MotionPlanRequest& req,
                                          const moveit::core::RobotState& rstate) const
{
  checkVelocityScaling(req.max_velocity_scaling_factor);
  checkAccelerationScaling(req.max_acceleration_scaling_factor);

If so, the issue is with the Pilz planner, not MTC (I strongly think so). File a bug report for MoveIt then.

[lkaising]

After investigating further, I can confirm that MTC (Move Task Constructor) is functioning correctly. Using a debugger to trace the scaling factor values, I set a breakpoint on TrajectoryGenerator::validateRequest with a relative motion stage set to 0.1:

(gdb) break TrajectoryGenerator::validateRequest
Breakpoint 1 at 0x7e98b28d0060 (2 locations)
(gdb) continue
Continuing.
...
(gdb) print req.max_velocity_scaling_factor
$4 = 0.10000000000000001
(gdb) print req.max_acceleration_scaling_factor
$5 = 0.10000000000000001

This confirms that the scaling factors are being properly conveyed through the system.

While investigating, I discovered this exact issue has been documented in MoveIt issue #3553. The fundamental problem lies in the Pilz planner's approach:

1. Applies velocity/acceleration scaling in Cartesian space
2. Converts to joint space via inverse kinematics
3. Checks joint limits last

This sequence explains why adjusting scaling factors doesn't resolve the issue - joint limits are verified too late in the process, after the trajectory has already been planned in Cartesian space.

Given these limitations, I'm considering alternative approaches:

1. Switching to MoveIt's Cartesian planner (though I initially moved away from it due to unrelated issues)
2. Exploring other planners that better handle moveRelative stages for fine linear motions

Do you have any suggestions for alternative planners or approaches that might better suit?

[rhaschke]

Do you have any suggestions for alternative planners or approaches that might better suit?

https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_cumotion