[pilz_industrial_motion_planner] velocity drop to near 0 during a plan_sequence_path service generated motion

[wuisky]

Description

Hi, I have a issue when executing a trajectory generated by pilz_industrial_motion_planner plan_sequence_path service.
Here is the motion generated by plan_sequence_path. There are two MotionSequenceItems in my MotionSequenceRequest(go to right 0.3[m], then go to down 0.3[m]. blend radius is 0.2[m]).

And i noticed that velocity dropped to near zero two times during the motion(at the time transition start and end?)

that causes an unsmooth motion.

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?

FastRTPS

Steps to Reproduce

pilz config file

planning_plugin: pilz_industrial_motion_planner/CommandPlanner
default_planner_config: PTP
request_adapters: >-
  default_planner_request_adapters/FixWorkspaceBounds
  default_planner_request_adapters/FixStartStateBounds
  default_planner_request_adapters/FixStartStateCollision
  default_planner_request_adapters/FixStartStatePathConstraints
response_adapters:
  - default_planning_response_adapters/ValidateSolution
  - default_planning_response_adapters/DisplayMotionPath
capabilities: >-
    pilz_industrial_motion_planner/MoveGroupSequenceAction
    pilz_industrial_motion_planner/MoveGroupSequenceService

pilz cartesian_limits:

cartesian_limits:
  max_trans_vel: 1.0
  max_trans_acc: 2.25
  max_trans_dec: -5.0
  max_rot_vel: 1.57

c++ code snippet

moveit_msgs::msg::MotionSequenceItem createMotionSequenceItem(
  const std::string group_name,
  const std::string link_name,
  const geometry_msgs::msg::Pose target_pose,
  const std::string planner_id,
  const double blend_radius)
{
  moveit_msgs::msg::MotionSequenceItem item;
  moveit_msgs::msg::Constraints constraints;
  moveit_msgs::msg::PositionConstraint position_constraint;
  position_constraint.header.frame_id = "world";
  position_constraint.link_name = link_name;
  item.req.group_name = group_name;
  item.req.planner_id = planner_id;
  item.req.max_velocity_scaling_factor = 0.1;
  item.req.max_acceleration_scaling_factor = 0.1;
  item.req.allowed_planning_time = 5.0;
  item.blend_radius = blend_radius;
  auto stamped_target_pose = [&target_pose] {
      geometry_msgs::msg::PoseStamped msg;
      msg.header.frame_id = "world";
      msg.pose = target_pose;
      return msg;
    }();
  item.req.goal_constraints.push_back(
      kinematic_constraints::constructGoalConstraints(link_name, stamped_target_pose));

  return item;
}

.....in main:

rclcpp::init(argc, argv);
auto const node = std::make_shared<rclcpp::Node>(
  "hello_moveit", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
auto spinner = std::thread([&executor]() {executor.spin();});

// Create the MoveIt MoveGroup Interface
const std::string arm_group = "ur_manipulator";
auto move_group_interface = MoveGroupInterface(node, arm_group);
const std::string end_effect_name = move_group_interface.getEndEffectorLink();

// create request
auto target_pose = move_group_interface.getCurrentPose().pose;
moveit_msgs::msg::MotionSequenceRequest seq_req;
moveit_msgs::msg::MotionSequenceItem item;
// point1
target_pose.position.y += 0.3;
item = createMotionSequenceItem(arm_group, end_effect_name, target_pose,  "LIN", 0.2); //0.2 is the blend radius
seq_req.items.push_back(item);
// point2
target_pose.position.z -= 0.3;
item = createMotionSequenceItem(arm_group, end_effect_name, target_pose,  "LIN", 0.0);
seq_req.items.push_back(item);

auto planning_client = node->create_client<moveit_msgs::srv::GetMotionSequence>("/plan_sequence_path");
auto request = std::make_shared<moveit_msgs::srv::GetMotionSequence::Request>();
request->request = seq_req;
auto future = planning_client->async_send_request(request);
auto result = future.get();
move_group_interface.execute(result->response.planned_trajectories[0]);
    

Expected behavior

Robot moves smoothly during the motion.

Actual behavior

Robot drops velocity to near zero two times.

Backtrace or Console output

[move_group-1] [ERROR] [1734055821.952725867] [moveit_robot_state.conversions]: Found empty JointState message
[move_group-1] [ERROR] [1734055821.952791463] [moveit_robot_state.conversions]: Found empty JointState message
[move_group-1] [ERROR] [1734055821.992271684] [moveit_robot_state.conversions]: Found empty JointState message
[move_group-1] [INFO] [1734055821.992606282] [moveit.pilz_industrial_motion_planner.trajectory_generator]: Generating LIN trajectory...
[move_group-1] [ERROR] [1734055821.992719494] [moveit_robot_state.conversions]: Found empty JointState message
[move_group-1] [INFO] [1734055822.000913702] [moveit.pilz_industrial_motion_planner.trajectory_generator]: Generating LIN trajectory...
[move_group-1] [INFO] [1734055822.008237715] [moveit.pilz_industrial_motion_planner.trajectory_blender_transition_window]: Start trajectory blending using transition window.
[move_group-1] [INFO] [1734055822.008276967] [moveit.pilz_industrial_motion_planner.trajectory_blender_transition_window]: Search for start and end point of blending trajectory.
[move_group-1] [INFO] [1734055822.008298902] [moveit.pilz_industrial_motion_planner.trajectory_blender_transition_window]: Intersection point of first trajectory found, index: 13
[move_group-1] [INFO] [1734055822.008310227] [moveit.pilz_industrial_motion_planner.trajectory_blender_transition_window]: Intersection point of second trajectory found, index: 22
[move_group-1] [INFO] [1734055822.013574717] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Received goal request
[move_group-1] [INFO] [1734055822.013841502] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution request received
[move_group-1] [INFO] [1734055822.013898132] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-1] [INFO] [1734055822.013953922] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-1] [INFO] [1734055822.014079221] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
[move_group-1] [INFO] [1734055822.015232349] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ...
[move_group-1] [INFO] [1734055822.015294652] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-1] [INFO] [1734055822.015327829] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-1] [INFO] [1734055822.015475473] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to joint_trajectory_controller
[ros2_control_node-3] [INFO] [1734055822.015956852] [joint_trajectory_controller]: Received new action goal
[ros2_control_node-3] [INFO] [1734055822.016075091] [joint_trajectory_controller]: Accepted new action goal
[move_group-1] [INFO] [1734055822.016319916] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: joint_trajectory_controller started execution
[move_group-1] [INFO] [1734055822.016354657] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[ros2_control_node-3] [INFO] [1734055826.962951209] [joint_trajectory_controller]: Goal reached, success!
[move_group-1] [INFO] [1734055826.967298954] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'joint_trajectory_controller' successfully finished
[move_group-1] [INFO] [1734055826.973286249] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ...
[move_group-1] [INFO] [1734055826.973395899] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution completed: SUCCEEDED

[sea-bass]

I wonder if it's related to #3158

[sea-bass]

Also to confirm, are you using a version of humble that includes this PR? #3000

[wuisky]

Thanks for reply.

Also to confirm, are you using a version of humble that includes this PR? #3000

yes, i am using the latest humble version which includes this PR.
Also, I have tried vivekcdavid:humble, but this problem is not resolved.

I have a new discovery, I plot the velocities in all JointTrajectoryPoint of result->response.planned_trajectories[0],
the velocity profile is more reasonable:

I am using joint_trajectory_controller of ros2_controllers.
As I know the reference JointTrajectoryPoint will be executed after some interpolation in the joint_trajectory_controller.
So I start to wonder the dropping of velocity is due to the joint_trajectory_controller.

[wuisky]

I think i figured out something.
For easier debugging, i chage planner_id from "LIN" to "PTP".
Left side is the profile of JointTrejactory msg(which is in RobotTrajectory msg that generated by plan_sequence_path service).

There are two "jumping" timestamp points. I think all the span between each time_from_start should be 0.1[sec] but there are two irregular points.
And after quintic spline interpolation in joint_trajectory_controller :https://github.com/ros-controls/ros2_controllers/blob/950c9c1a056c8d8861000a8f11facb7e1cd9f8c9/joint_trajectory_controller/src/trajectory.cpp#L295 the actual reference control_state becomes the profile in right side of this picture.
This cause the dropping of the velocities.

I will trying to find the reason of the "jumping" timestamp.

[sea-bass]

It appears that when the right side drops to zero, the time between waypoints on the left (reference) is slightly stretched out. Maybe the controller "misses" a time step and defaults to zero instead of interpolating? Not sure.

Is there a ROS 2 control setting to affect this behavior, or does the trajectory need to be resampled in some way? I'm not sure

[wuisky]

OK, finally i figured out what's happened.

There is an unnecessary time offset here:

moveit2/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp

Line 65 in f91a650

result.append(source, source.getWayPointDurationFromStart(0));

When concatenating blending trajectories here

moveit2/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp

Line 101 in f91a650

appendWithStrictTimeIncrease(*(traj_cont_.back()), *blend_response.blend_trajectory);

the first waypoint of the blend_trajectory do has a duration_from_previous 0.1,
and duration_from_previous will be used for calculated time_from_start in this function:

moveit2/moveit_core/robot_trajectory/src/robot_trajectory.cpp

Line 265 in f91a650

void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& trajectory,

With an unnecessary time offset, the first waypoint of an blended trajectory in time t is shifted to time t+0.1,
and that causes a unexpected velocity dropping when using joint_trajectory_controller.

After i get rid of the unnecessary time offset, I got my motion profile:

The velocity dropping is resolved.
I'll make a PR for showing my revision.

[sea-bass]

Interesting. I had added this to get rid of some duplicate trajectory points that were causing errors on Rolling in #2961

Your fix may work, so long as it doesn't break what originally got fixed. I will need to test.

[sea-bass]

Posting on the issue as well as the PR for visibility.

It seems that the fixes in #2961 were conditioned on #1813 also having been backported to Humble. This seems to be why Pilz sequence blending worked fine on Humble but not on later versions.

Indeed, it was not ros2_control that suddenly started checking for duplicate waypoints, but that the RobotTrajectory::append() implementation differs on Humble vs. later versions of MoveIt.

So I am fairly confident that merging the backport in #1821 will resolve this particular issue -- but will wait to confirm.

[sea-bass]

Closed by #1821