Cannot use blending feature with pilz_industrial_motion_planner

[lokneey]

Description

Hi,
I try to run pilz planner on ur_robot_driver package. I managed to successfully move robot with simple action server /sequence_move_group. However it works only without blending feature. I can successfully move robot with PTP and LIN planner. When I add blend radius other than 0 to each of them I get error presented below. As far as I know it comes from this place in code: trajectory_blender_transition_window.cpp#L164
Is there any bug in validation of start and end positions of computed trajectories? As is said in tutorial I specify start state only for first point. I specify points via joints positions.

Your environment

• ROS Distro: Melodic
• OS Version: e.g. Ubuntu 18.04 LTS
• Binary build
• Latest version
• If source, which branch?

Steps to reproduce

Simple fulfilling planning requests as is in tutorial.

Expected behaviour

Should plan and execute trajectory with blended one or more points.

Actual behaviour

Cannot pass through blending process.

Backtrace or Console output

[ WARN] [1634200132.831646107]: Execution of motions should always start at the robot's current state. Ignoring the state supplied as difference in the planning scene diff
[ INFO] [1634200132.832058001]: Planning attempt 1 of at most 1
[ INFO] [1634200132.832397198]: Initialized Point-to-Point Trajectory Generator.
[ INFO] [1634200132.832454325]: Generating PTP trajectory...
[ INFO] [1634200132.836820501]: Initialized Point-to-Point Trajectory Generator.
[ INFO] [1634200132.836875984]: Generating PTP trajectory...
[ INFO] [1634200132.858628022]: Initialized Point-to-Point Trajectory Generator.
[ INFO] [1634200132.858675209]: Generating PTP trajectory...
[ INFO] [1634200132.869543908]: Initialized Point-to-Point Trajectory Generator.
[ INFO] [1634200132.869606840]: Generating PTP trajectory...
[ INFO] [1634200132.891174283]: Initialized Point-to-Point Trajectory Generator.
[ INFO] [1634200132.891233685]: Generating PTP trajectory...
[ INFO] [1634200132.896357055]: Initialized Point-to-Point Trajectory Generator.
[ INFO] [1634200132.896408556]: Generating PTP trajectory...
[ INFO] [1634200132.898447008]: Start trajectory blending using transition window.
[ERROR] [1634200132.898497624]: During blending the last point of the preceding and the first point of the succeding trajectory
[ERROR] [1634200132.898520577]: Trajectory blend request is not valid.
[ERROR] [1634200132.898865621]: Planning pipeline threw an exception (error code: 99999): Blending failed

[welcome]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[jschleicher]

@lokneey Can you come up with a minimum example? How do you prepare the request? Did you try using the python helpers in PilzDE/pilz_robot_programming? Using PRBT config and demo program, the blending feature seems to work as expected:

roslaunch prbt_moveit_config moveit_planning_execution.launch pipeline:=pilz_industrial_motion_planner
rosrun pilz_robot_programming demo_program.py

Log output:

[ INFO] [1634907319.592019291]: Start trajectory blending using transition window.
[ INFO] [1634907319.592044030]: Search for start and end point of blending trajectory.
[ INFO] [1634907319.592065193]: Intersection point of first trajectory found, index: 31
[ INFO] [1634907319.592081418]: Intersection point of second trajectory found, index: 4
[ INFO] [1634907319.691086432]: Fake execution of trajectory
[ INFO] [1634907324.990219913]: Completed trajectory execution with status SUCCEEDED ...

[rr-tom-noble]

I seem to remember having the same issue planning with PTP, though the points in my paths are very closely spaced, so I'm unsure if I'm just choosing too large of a value. I'll try to get a video uploaded showing more details unless @lokneey beats me to it 😉

[lokneey]

Hi all,
@jschleicher I am using C++ following official moveit tutorial where they advice to communicate with /sequence_move_group action server. I also tried /plan_sequence_path service with same result. I fill request like in example below.

    actionlib::SimpleActionClient<moveit_msgs::MoveGroupSequenceAction>sequence_planning_action_client("/sequence_move_group", true);

    moveit::core::robotStateToRobotStateMsg(*move_group.getCurrentState(), robot_state_msg, true);


    global_plan_item.req.group_name = PLANNING_GROUP;
    global_plan_item.req.allowed_planning_time = 1.5;
    global_plan_item.req.max_acceleration_scaling_factor = 0.5;  // 0.05 also fails
    global_plan_item.req.max_velocity_scaling_factor = 0.5;  // 0.05 also fails
    global_plan_item.req.workspace_parameters.max_corner.x = 2.5;
    global_plan_item.req.workspace_parameters.max_corner.y = 2.0;
    global_plan_item.req.workspace_parameters.max_corner.z = 2.0;
    global_plan_item.req.workspace_parameters.min_corner.x = -2.5;
    global_plan_item.req.workspace_parameters.min_corner.y = -2.0;
    global_plan_item.req.workspace_parameters.min_corner.z = -2.0;
    global_plan_item.req.planner_id = "PTP"; // "LIN" also fails
    global_plan_item.blend_radius = 0;

    joint_values = {-3.14, -1.22, -1.92, -1.57, 1.57, 0.0};
    pose_goal_state.setJointGroupPositions(joint_model_group, joint_values);
    joint_goal = kinematic_constraints::constructGoalConstraints(pose_goal_state, joint_model_group);
    req_test.goal_constraints.clear();
    req_test.goal_constraints.push_back(joint_goal);

    global_plan_item.req.goal_constraints.clear();
    global_plan_item.req.goal_constraints.push_back(joint_goal);

    global_plan_item_first_point = global_plan_item;   
    global_plan_item_first_point.req.start_state = robot_state_msg;

    global_plan_and_execute_action.action_goal.goal.request.items.push_back(global_plan_item_first_point);


    joint_values = {-2.10, -1.39, -1.51, -1.27, 1.52, 0.06};
    pose_goal_state.setJointGroupPositions(joint_model_group, joint_values);
    joint_goal = kinematic_constraints::constructGoalConstraints(pose_goal_state, joint_model_group);
    req_test.goal_constraints.clear();
    req_test.goal_constraints.push_back(joint_goal);
    global_plan_item.blend_radius = 0.1; // 0.01 also fails

    global_plan_item.req.goal_constraints.clear();
    global_plan_item.req.goal_constraints.push_back(joint_goal);

    global_plan_and_execute_action.action_goal.goal.request.items.push_back(global_plan_item);


    joint_values = {-1.36, -1.33, -0.91, -1.03, 1.76, 0.04};
    pose_goal_state.setJointGroupPositions(joint_model_group, joint_values);
    joint_goal = kinematic_constraints::constructGoalConstraints(pose_goal_state, joint_model_group);
    req_test.goal_constraints.clear();
    req_test.goal_constraints.push_back(joint_goal);

    global_plan_item.req.goal_constraints.clear();
    global_plan_item.req.goal_constraints.push_back(joint_goal);
    global_plan_item.blend_radius = 0; 

    global_plan_and_execute_action.action_goal.goal.request.items.push_back(global_plan_item);

    sequence_planning_action_client.sendGoal(global_plan_and_execute_action.action_goal.goal);

    bool finished_before_timeout = sequence_planning_action_client.waitForResult(ros::Duration(30.0));

    if (finished_before_timeout)
    {
        actionlib::SimpleClientGoalState state = sequence_planning_action_client.getState();
        ROS_INFO("Action finished: %s", state.toString().c_str());
    }
    else
    {
        ROS_INFO("Action did not finish before the time out.");
    }

Example with commands that you provide is passing through with blending, but fails in some stage after, because of violation of joint limit. The problem is that example in python is simplified a lot in comparison to C++ version and I am also not sure if it actually uses the same interface as is recommended in MoveIt tutorials.

Do I miss some definition?

[mauderliEmbotech]

In case this helps anyone: I got the same error message when planning a motion sequence with the LIN planner of the pilz_industrial_motion_planner::CommandPlanner plugin. It turns out I had the default_planner_request_adapters/AddTimeOptimalParameterization enabled and disabling it fixed the issue for me. Note that my machine was running Ubuntu 20.04 with noetic.

The debug logs indicate that (if the AddTimeOptimalParametrization adapter is enabled) the planning adapters are invoked after planning each trajectory segment. Of course, this will overwrite the velocities and accelerations of every trajectory segment. In the following lines of code, this will cause the checks in pilz_industrial_motion_planner::isRobotStateEqual to fail:

https://github.com/ros-planning/moveit/blob/7855e1ffb9e1d6a1b549e223bfde01ec22bd2e22/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp#L167-L173

Find the logs below:

[DEBUG] [1666116979.321981264]: Running 'Add Time Optimal Parameterization'
[DEBUG] [1666116979.326154687]: Motion planner reported a solution path with 33 states
[DEBUG] [1666116979.327849165]: Planned path was found to be valid when rechecked
[DEBUG] [1666116979.327920200]: Solved [3/3]
[ INFO] [1666116979.327995407]: Start trajectory blending using transition window.
[DEBUG] [1666116979.328011188]: Validate the trajectory blend request.
[DEBUG] [1666116979.328049924]: Joint accelerations of the two states are different. state1: -0.00170157
    0.68053
   0.318741
         -1
 0.00647264
 0.00319807 state2: 0
0
0
0
0
0
[ERROR] [1666116979.328071338]: During blending the last point of the preceding and the first point of the succeding trajectory
[ERROR] [1666116979.328084278]: Trajectory blend request is not valid.
[ERROR] [1666116979.328184473]: Planning pipeline threw an exception (error code: 99999): Blending failed
[DEBUG] [1666116979.328211082]: PlanExecution terminating with error code 99999 - 'Catastrophic failure'

I understand that joint positions are checked in pilz_industrial_motion_planner::isRobotStateEqual but I don't see any reason to check velocities and accelerations. Wouldn't it be sufficient to only check joint positions?