[BUG] Segmentation Fault caused by Dart TSR Error Calculation

[ell0ry]

When adding a TSR specification as a constraint for a top-down grasp, a segmentation fault will be thrown at runtime 60-70% of the time. This issue emerged when I was modifying the fetch_pick.cpp example script in robowflex_dart. I changed the constraint tsr specification to have the "base_link" as base frame, "wrist_roll_link" as target frame, and to get pose from the world with no z position tolerance. The goal specification for this planning query is specified in the cube's frame (and not in the robot's frame).

Expected Behavior

Expected the framework to plan successfully while constrained to the x and y position of the cube.

Current Behavior

A segmentation fault is thrown 60-70% of the time. Compiling with debug symbols and running with gdb shows that the error is coming from robowflex_dart/src/tsr.cpp:582:

auto tsr_error = tsr_->computeError();

Changing the TSR specification constraint to be in the same frame as the goal specification seems to have resolved this issue.

Possible Solution

The stack trace reports the error to originate from the TSR::getErrorWorld method. This method locks the current world pointer, calls the DART tsr pointer to compute its error, adds that error to the current TSR error, and then unlocks the world pointer. To determine if the error was being caused by faulty locks or null DART tsr pointers, I added print statements before and after each call.

It seems as if this error is caused by the DART tsr pointer directly. The world lock always seems to succeed and prior world locks all seem to be unlocked (eliminating any potential race conditions). Additionally, printing out the DART tsr shows that it isn't null.

Here are some screenshots of the script failing:

Here are some screenshots of the script succeeding:

Steps to Reproduce

1. Checkout the tsr_error branch of robowflex_mmp into a catkin workspace with robowflex and ompl.
2. Compile with catkin build --cmake-args -DCMAKE_BUILD_TYPE=Debug to add debug.
3. Run gdb on executable found in catkin workspace's devel/lib/robowflex_mmp directory.

Context (Environment)

This error popped up while attempting to integrate mmp work in OMPL and robowflex_dart. From examining the root cause, it doesn't seem that this is an OMPL issue but be aware that this issue did use a non-standard version of OMPL.