last waypoint from STOMP solution doesn't always match goal

[patrickKXMD]

Description

this is a really annoying problem, the last waypoint from STOMP solution doesn't always match the goal_state

I added four lines here for debug
https://github.com/ros-planning/moveit2/blob/c2292a7054904c25851b6af78abb6aca8a5852ae/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp#L68C1-L71C4

  if (!input_trajectory || input_trajectory->empty())
  {
    success = stomp->solve(getPositions(start_state, joints), getPositions(goal_state, joints), waypoints);
    // new lines
    auto goal_positions = get_positions(goal_state, joints);
    auto last_waypoint = waypoints.col(waypoints.cols() - 1);
    RCLCPP_WARN(LOGGER, "TEST Goal position: %.12f, %.12f, %.12f, %.12f, %.12f, %.12f", goal_positions[0], goal_positions[1], goal_positions[2], goal_positions[3], goal_positions[4], goal_positions[5]);
    RCLCPP_WARN(LOGGER, "TEST Last waypoint: %.12f, %.12f, %.12f, %.12f, %.12f, %.12f", last_waypoint[0], last_waypoint[1], last_waypoint[2], last_waypoint[3], last_waypoint[4], last_waypoint[5]);
  }

and here is the output

sometimes they are identical which is good.

[stomp_moveit]: TEST Goal position: 0.234587246836, 0.992731932008, -2.384229104531, -0.235384185478, -2.907002753126, -0.000018023283
[stomp_moveit]: TEST Last waypoint: 0.234587246836, 0.992731932008, -2.384229104531, -0.235384185478, -2.907002753126, -0.000018023283
[stomp_moveit]: TEST Goal position: 1.560993985891, 0.992730570867, -2.384225161300, -0.235366751959, -1.580596014100, -0.000002689587
[stomp_moveit]: TEST Last waypoint: 1.560993985891, 0.992730570867, -2.384225161300, -0.235366751959, -1.580596014100, -0.000002689587

sometimes they are different

[stomp_moveit]: TEST Goal position: 0.975131363602, 0.760374503146, -0.912535765952, 1.468677947059, -2.166458636384, -0.000005143243
[stomp_moveit]: TEST Last waypoint: 0.975118794469, 0.760292204555, -0.912173806359, 1.469008654541, -2.166498134031, 0.000181114966
[stomp_moveit]: TEST Goal position: 0.234587246836, -0.235403262503, -2.186147346800, 1.190832766764, -2.907002753126, -0.000018023283
[stomp_moveit]: TEST Last waypoint: 0.234464227906, -0.235237264322, -2.185758444412, 1.191559432882, -2.907126802335, 0.000037962755

I don't know why this happen but it's really annoying

Your environment

• ROS Distro: Humble
• OS Version: Ubuntu 22.04
• Source build tags/2.8.0
• Cyclone DDS

Steps to reproduce

Using STOMP planner and plan for some specific joint values

Expected behaviour

The last waypoint should be identical with the goal_state

Backtrace or Console output

Use gist.github.com to copy-paste the console output or segfault backtrace using gdb.

[henningkayser]

@patrickKXMD could you test if #2625 fixes this issue for you?

[patrickKXMD]

@patrickKXMD could you test if #2625 fixes this issue for you?

Hi @henningkayser
Your commit conflicts with tags/2.8.0 so I shall test this on main branch of moveit2.

But the stomp planner always failed to plan in allowed time (which is 10 seconds) for my case on the latest main branch and I'm still investigating...

[patrickKXMD]

TEST Goal position: 1.560993985891, 0.992730570867, -2.384225161300, -0.235366751959, -1.580596014100, -0.000002689587
TEST Last waypoint: 1.561182962212, 0.993394494560, -2.384768087164, -0.235530850700, -1.580864178244, 0.000388942661
TEST Goal position: 0.234587246836, 0.992731932008, -2.384229104531, -0.235384185478, -2.907002753126, -0.000018023283
TEST Last waypoint: 0.234587246836, 0.992731932008, -2.384229104531, -0.235384185478, -2.907002753126, -0.000018023283
TEST Goal position: 0.975131363602, 0.760374503146, -0.912535765952, 1.468677947059, -2.166458636384, -0.000005143243
TEST Last waypoint: 0.974491463725, 0.760383713959, -0.912977762658, 1.468600399367, -2.166632856705, 0.000251513712

@henningkayser look like it's not fixed, sometimes the goal and last waypoint is not identical

[github-actions]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[moccij]

I too have the same problem, both starting and goal states can differ greatly from those specified in the MotionPlanRequest, even when planning trivial paths without collisions.
I am using ROS2 Humble and the latest STOMP planner MoveIt implementation (2.10.0)