Strange acceleration results

[mxgrey]

I've managed to find a few examples which demonstrate the strange behavior I had mentioned yesterday. I wasn't able to come up with any examples manually, so I had Paul try to generate some (because the behavior was first witnessed while testing the integration his planner with my framework). As far as I know, Paul is generating the trajectories using the RRT path planner and path shortening packages which come with DART.

I've made a fork of the trajectories repo and put the three examples that I've found in there.

The repo is here: https://github.com/mxgrey/trajectories And the source for the example trajectories are here: https://github.com/mxgrey/trajectories/blob/master/Example.cpp

You should be able to copy/paste my Example.cpp into your own build or just clone my repo to see the behavior.

Running the Example program with an argument of "1" (or no argument at all, because 1 is the default) should print this out at the bottom:

Joint (0) violated its max acceleration (0.8) with a value of 271.306 at index 287! Joint (1) violated its max acceleration (0.8) with a value of 96.4466 at index 287! Joint (2) violated its max acceleration (0.8) with a value of 162.626 at index 287! Joint (3) violated its max acceleration (0.8) with a value of 367.439 at index 287! Joint (0) violated its max acceleration (0.8) with a value of 24.0424 at index 288! Joint (1) violated its max acceleration (0.8) with a value of 8.54695 at index 288! Joint (2) violated its max acceleration (0.8) with a value of 14.4115 at index 288! Joint (3) violated its max acceleration (0.8) with a value of 32.5613 at index 288!

In all three cases that I've documented so far, it seems that the acceleration is only crazy for two consecutive time steps, but during the initial tests there were clearly times where a single trajectory would have multiple separate instances of strange accelerations.

I have no idea what it is about these particular trajectories which is resulting in problematic accelerations. Examples 1 and 2 are similar in the sense that they start and end at almost the same waypoint, but Example 3 breaks that trend.

Let me know if there's anything I can do to help debug this. For right now, I'm going to move on to implementing a cubic spline-based interpolation and then revisit this when I've finished that.

[tobiaskunz]

I looked at your first example. The problem is caused by the first 3 waypoints. From waypoint 2 to 3, you are going back in exactly the direction you came from between waypoints 1 and 2. As discussed in issue #1 this is currently not supported.

How did you generate this list of waypoints? I doubt that it was generated using an RRT planner. The likelihood of the RRT planner generating waypoints that result in a 180 degree turn is zero. This is why I did not bother implementing this case so far.

[mxgrey]

The path planner's trajectory gets prepended with the current joint configuration of the physical robot, in case the planner did not have exact knowledge of the robot's actual configuration. In other words, you're right that this first point is not being generated by the RRT & shortener.

So what is probably happening is the mismatch between the planned path and the starting configuration results in something which is numerically very close to a 180 turn. It doesn't seem to be exact, but difference is on the order of 1e-7, so maybe that's enough to produce numerical instability.

In simple test cases, given a perfect 180 turn, your trajectory generator usually fails an Eigen assertion because the uninitialized variable "activeConstraint" in getVelocityMaxPathVelocity() remains uninitialized. But this doesn't happen in the cases that I'm experiencing, which leads to me to two possible conclusions:

1) The numerical precision allows activeConstraint to get set, but it's numerically unstable. 2) activeConstraint is never actually set, and it's not triggering the Eigen assertion because the arbitrary initial value that it's given at declaration happens to be within the size of the vector.

Possibility (2) is easily dealt with by initializing activeConstraint as unsigned int activeConstraint = (unsigned int)(-1); That will ensure that the Eigen assertion fails if activeConstraint never gets set within the for loop. Although rather than letting the Eigen assertion trigger, I think it would be preferable to check the value of activeConstraint after the for loop and give up while returning an error if it's equal to (unsigned int)(-1).

For possibility (1), it would probably be good to check whether the values are within the tolerance (whatever that happens to be), and then return an error if they're not.

I'll be making these adjustments to source that I've pulled, and I'll let you know if it doesn't resolve the problems.

[tobiaskunz]

Can you explain why the planner does not have exact knowledge of the start configuration?

Looking at the example it seems your problem is setup such that the planner finds a straight-line path between the start and goal configurations. You are then trying to execute that path while the robot is at the goal configuration. Why are you not first moving the robot back to the start configuration?

[tobiaskunz]

You can also adapt the code to be able to deal with this case. But that might require very careful analysis and design of the code. I can't say anything about the code details you mentioned above right now.

[mxgrey]

For examples 1 and 2, I was basically instructing the robot to restart the path after it had been executed once. This obviously has inherent risks and is not something that would normally be done; I was just desperately trying to recreate the strange results that I had been getting the other day.

For example 3, I was instructing it to start on a different trajectory while it was in the middle of executing one (i.e. interrupting the current trajectory to execute a new one). This is the more likely case to happen in real life. If I start to re-plan from where the robot currently is in the trajectory, then the robot will have certainly moved some amount before the plan has finished generating, in which case we would want to attach the current waypoint onto the front of the trajectory. Again, this is obviously not an ideal scenario, but I still think it's good to have the safety measure.

For the time being, I will implement the sanity checks that I mentioned in the previous post, and then in my own code I will inspect all the waypoints and partition them anywhere that a 180-degree turn occurs. I hope that will resolve the issues I've experienced with minimal effort on everyone's part.