NaN explosion when the angle between consecutive segments is 180-degrees

[mxgrey]

I found that the optimizer has issues with trajectories which go to a point and then return to the starting point. A simple example would be

waypoint << 0.0, 0.0, 0.0; waypoint << 1.0, 0.0, 0.0; waypoint << 0.0, 0.0, 0.0;

Obviously the right thing to do would be go to the middle point, come to a complete stop for an instant, and then go to the final point. But instead an assertion gets thrown because it can't figure out which joint should have the active constraint (and therefore the uninitialized variable "activeConstraint" in the function getVelocityMaxPathVelocityDeriv() is going to have some garbage value which is likely to be out-of-bounds).

So far what I've observed is that the "start point" for the final LinearPathSegment is "-nan, -nan, -nan" and the path segment's length is also nan. This makes me suspect that the endpoint of the circular blend is being calculated as nan for some reason.

I'll try to think of an easy solution for this, but I don't have any great ideas off the top of my head. Maybe there would need to be a third type of PathSegment to handle this special case?

[tobiaskunz]

This is currently not supported. You have to detect this case yourself, split the path in two segments and call the trajectory generation for each segment individually.

[mxgrey]

I see.

On an unrelated note, as I've been digging through the code, I've noticed "abs()" used in a lot of places. Maybe different platforms have different implementations of abs(), but on at least Linux, abs() returns an integer value. fabs() is the function that should be used for doubles. This might cause some bad behavior if a double was expected to be returned.

And on the note of unexpected behavior, I've been running into scenarios where I have it generate a trajectory from some waypoints which came out of a planner, and it successfully generates... but when I grab a dense set of evenly distributed sample point and do some finite difference checks on their velocity and acceleration values, the acceleration values are rather bad. The velocity values are fine, but the acceleration values can sometimes be on the order of 50-500 (rad/s^2) when the instructed max was 1 (rad/s^2). And the nasty acceleration is very noticeable when running it on the robot, because the robot will shake somewhat violently.

Unfortunately, I have not managed to intentionally reproduce it the problem, but that's what I'm trying to do right now, which is how I ran into the situation which prompted this thread originally. Once I've managed to find a test case which consistently does what I've described, I'll let you know.

[tobiaskunz]

The C++ standard also defines

double std::abs(double)

Yea, a demo case where the described problem appears would be helpful.

[mxgrey]

I'm having an extremely hard time intentionally making the problem happen, so for now I'll assume that it was user error on my end, and I'll just be prepared to log the incident the next time it happens. And then if the issue is able to be recreated outside of my framework, then I'll report it as a separate issue.

Thanks!