Random crash with STOMP with path constraints

[patrickKXMD]

Description

There is a random SIGSEGV when using STOMP with path constraints in my project.

Here is the backtrace

#0 0x0000ffff9a708298 in ?? () from /lib/aarch64-linux-gnu/libfcl.so.0.7
#1 0x0000ffff9a7085c0 in ?? () from /lib/aarch64-linux-gnu/libfcl.so.0.7
#2 0x0000ffff9a708658 in ?? () from /lib/aarch64-linux-gnu/libfcl.so.0.7
#3 0x0000ffff9a708658 in ?? () from /lib/aarch64-linux-gnu/libfcl.so.0.7
#4 0x0000ffff9a704f70 in fcl::DynamicAABBTreeCollisionManager::registerObjects(std::vector<fcl::CollisionObject, std::allocator<fcl::CollisionObject> > const&) ()
from /lib/aarch64-linux-gnu/libfcl.so.0.7
#5 0x0000ffff9acf0094 in collision_detection::FCLObject::registerTo (
this=this@entry=0xffff4de40be8, manager=0xffff38009880)
at /.../src/external/ros-planning/moveit2/moveit_core/collision_detection_fcl/src/collision_common.cpp:997
#6 0x0000ffff9acfe0ec in collision_detection::CollisionEnvFCL::allocSelfCollisionBroadPhase (
this=this@entry=0xffff380237f0, state=..., manager=...)
at /.../src/external/ros-planning/moveit2/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp:254
#7 0x0000ffff9acfe278 in collision_detection::CollisionEnvFCL::checkSelfCollisionHelper (
this=0xffff380237f0, req=..., res=..., state=..., acm=0xffff38009970)
at /.../src/external/ros-planning/moveit2/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp:274
#8 0x0000ffff9b2d30a8 in planning_scene::PlanningScene::isStateColliding (this=0xffff38018840,
state=..., group=..., verbose=)
at /.../src/external/ros-planning/moveit2/moveit_core/planning_scene/src/planning_scene.cpp:2049
#9 0x0000ffff4feee88c in stomp_moveit::get_positions (state=...,
joints=std::vector of length -155488448280990176, capacity -155664653690366117 = {...})
at /.../src/external/ros-planning/moveit2/moveit_planners/stomp/include/stomp_moveit/conversion_functions.hpp:59

I may have found the root cause after some digging.

https://github.com/ros-planning/moveit2/blob/9249e44330d241559ed868dbc293fc2a3d1e1907/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp#L124-L128
here when invalid_windows.back().first is 0 and timestep is 1

https://github.com/ros-planning/moveit2/blob/9249e44330d241559ed868dbc293fc2a3d1e1907/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp#L139-L147
mu = 0.5 * (start + end) will be 0 and costs(j) became inf which is invalid for later use and finally cause the crash.

I can workaround it but don't know how to fix it properly.
please let me know if I'm wrong, thanks.

Your environment

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

[henningkayser]

interesting... first off, these functions are still lacking proper testing. But it seems like you are starting with a start state that doesn't meet the path constraints (0,1 being an invalid window). Usually, this should have been caught by the plugin adapter without actually running the algorithm. What is your input trajectory size (timestep count)? Also, number of dof? Line 9 in the stacktrace looks somewhat suspicious, though...

[patrickKXMD]

@henningkayser , thanks for your comment.
sorry I'm not familiar with the source code, the backtrace shows error happens when calling planning_scene->isStateColliding(state, group_name) and I found that state is full of nan, and these nans are caused by the code I mentioned.

In my case it's a 6-DOF arm in simulation.
The path constraint contains a PositionConstraint which is a cube with start pose of eef in one corner and target pose in another in diagonal. I want to make sure the eef move inside the cube.
So in theory the start state should meet the constraint? or I could add extra paddings to the cube to ensure eef is inside.

What is your input trajectory size (timestep count)?

I'm using standalone STOMP planning (not as a post-processor). values.cols() shows 40 so I guess the timestep count is 40.

[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.

[henningkayser]

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

[patrickKXMD]

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

I ran my test based on your commit and no crash happens yet
look like you got it fixed, thanks