[bug] spline velocity multiplication perturbs spline

[ddengster]

https://github.com/osrf/rmf_core/blob/master/rmf_traffic/src/rmf_traffic/Spline.cpp#L114

This LOC shows a scalar multiplication to velocities of the spline and therefore perturbs the spline when used for collision. The current workaround is to set trajectories to 1s.

---

Given a trajectory like this, and submitting it into the DetectConflict routine:

rmf_traffic::Trajectory t2;
t2.insert(time, Eigen::Vector3d(-2, 2, 0), Eigen::Vector3d(0, -2, 0));
t2.insert(time + 10s, Eigen::Vector3d(2, 2, 0), Eigen::Vector3d(0, 2, 0));

CHECK(rmf_traffic::DetectConflict::between(profile_circle, t1, profile_circle_with_circle_offset, t2));

This flows into the detect_invasion function where we start construct the spline https://github.com/osrf/rmf_core/blob/master/rmf_traffic/src/rmf_traffic/DetectConflict.cpp#L505.

This then flows into the compute_parameters function https://github.com/osrf/rmf_core/blob/master/rmf_traffic/src/rmf_traffic/Spline.cpp#L97 where delta_t multiples the start and end velocities resulting in (0,-20,0) and (0,20,0).

to_fcl is then called https://github.com/osrf/rmf_core/blob/master/rmf_traffic/src/rmf_traffic/Spline.cpp#L259 and it flows into the compute_knots function https://github.com/osrf/rmf_core/blob/master/rmf_traffic/src/rmf_traffic/Spline.cpp#L222 where we compute v0 and v1, which turn out to be (0,-20,0) and (0,20,0). These are plugged into the conversion to fcl splines, and give me incorrect results when used for collision.

we should divide the v0 and v1 by delta_t in the compute_knots function; but what perplexes me is the need to multiply the velocities of the spline in the first place.

[mxgrey]

These are plugged into the conversion to fcl splines, and give me incorrect results when used for collision.

You haven't included enough code to demonstrate an incorrect continuous collision detection result.

As for the rest of the matter, the first thing to note is that the API of FCL is specifically designed to operate over a time range of t=[0.0, 1.0]. If we have any time range other than [0.0, 1.0], then we need to transform the spline coefficients into an equivalent spline that has that time range.

The simplest way to justify the transformation is to say "This is how FCL was made, so our hands are tied and we have to do this transformation." But looking at it holistically, we'd want to do this transformation anyway for our own benefit. Taking the position and velocity end conditions and transforming them into cubic spline coefficients is far far easier if you first transform your time coordinate to fit within the range of t=[0.0, 1.0]. I'm sure this is why FCL chose to have this constraint itself.

To understand the reason this multiplication needs to happen, we have to look at some vector calculus, which I'll simplify here.

Let's suppose x(t) is our function of position over time (we'll think of x as a scalar value rather than a vector value for simplicity, but the following math is all the same for vector values as well).

For x(t), the valid range of t is t=[ti, tf] where ti is the initial time of our spline and tf is the final time of our spline.

We need to transform this into a new function ξ(u) where u=[0.0, 1.0].

We want a continuous linear mapping from the values of u to the values of t. We'll call this mapping τ(u) where τ(0) = ti and τ(1) = tf. Since this is a linear mapping, we can easily derive the whole formula: τ(u) = (tf - ti)u + ti

With this mapping we can define ξ(u) := x(τ(u))

To calculate the coefficients of the spline ξ(u), we'll need its endpoint conditions, meaning ξ(0), ξ(1), ξ'(0), and ξ'(1). The positions are trivial:

ξ(0) = x(τ(0)) = x(ti) ξ(1) = x(τ(1)) = x(tf)

ξ(u) has the same endpoint positions as x(t). However, let's look at its velocities. First let's take the derivative of ξ(u):

ξ'(u) = d[ x(τ(u)) ] / du = d[ τ(u) ]/du d[ x(τ(u)) ]/dτ = τ'(u) x'(τ(u))

We can compute τ'(u) from its formula above: τ'(u) = tf - ti

So this means that the velocity endpoint conditions for ξ'(u) are:

ξ'(0) = (tf - ti) x'(τ(0)) = (tf - ti) x'(ti) ξ'(1) = (tf - ti) x'(τ(1)) = (tf - ti) x'(tf)

In other words, the velocities at the endpoints of ξ(u) are equal to the velocities at the endpoints of x(t) multiplied by the time range of x(t).

I can't rule out that an implementation error may have happened somewhere in the code, but multiplying by Δt is mathematically sound here. You're going to need to offer more analysis if you really believe there's an error.

[ddengster]

I think there's a difference in that the code computes knots for a new, different spline, and it does not map ranges onto the existing spline like what's being described.

The following picture features a green spline with starting velocity (0,0,-2) and ending velocity (0,0,2),

And this picture features a green spline with starting velocity (0,0,-20) and ending velocity (0,0,20)

You can see that there's a huge difference.

Regarding more sample code, give me some time to clean up the test cases.

[ddengster]

Alright, here are the test cases.

Case 1: rmf_core Clone these branches into your workspace: https://github.com/osrf/rmf_core/tree/robot_multigeom_showcase https://github.com/ddengster/fcl/tree/0.6.1-fix-conservative-adv

then run clear && cb --packages-up-to rmf_traffic --cmake-args -DCMAKE_BUILD_TYPE=Release && ./build/rmf_traffic/test_rmf_traffic

You will see a test failure at line 88. The test case showcases a stationary robot vs a robot going through the DetectConflict function. Both trajectories are computed at 10 seconds offset, if you change it to 1 second you should get the same results as test case 2.

Case 2: fcl Clone this branch: https://github.com/ddengster/fcl/tree/bug_showcase

then build fcl and run the test

mkdir build
cd build
make test_fcl_collision
./test/test_fcl_collision

Check the function test_conversative_advancement_with_offset in the file test/test_fcl_collision.cpp We have a couple of functions copied in from rmf_traffic - compute_knots and to_fcl. I start with the exact same points and run the collision test.

Now even if the test 2 doesn't comply with the 10 second trajectory case, it should be known that given test case 1's code, and verified by visualizations I should expect a collision.

[mxgrey]

I can't know for sure, but based on how both splines are being displayed at t=0.5, I very strongly suspect both of these splines have a default time range of t=[0.0, 1.0]. In that case, yes, changing the velocity will change the curvature of the spline.

For your visualizer to be applicable here, you need it to be able to work over an arbitrary time range, and then you need to compare the spline with velocities of +/-20 with a time range of t=[0.0, 1.0] to a spline with velocities of +/-2 with a time range of t=[0.0, 10.0].

For the spline with a time range of t=[0.0, 1.0] you need to check the spline location at t=0.5. For the time range of t=[0.0, 10.0] you need to check the spline location at t=5.0.

[ddengster]

The simplest way to justify the transformation is to say "This is how FCL was made, so our hands are tied and we have to do this transformation." But looking at it holistically, we'd want to do this transformation anyway for our own benefit. Taking the position and velocity end conditions and transforming them into cubic spline coefficients is far far easier if you first transform your time coordinate to fit within the range of t=[0.0, 1.0]. I'm sure this is why FCL chose to have this constraint itself.

If this is a remapping, why wouldn't a simple variable range remapping t suffice?

As I see it, the code does this: 1) User decides on points and velocities for a cubic hermite spline 2) The code computes points and velocities at start and end times. 2) the code converts those points to knots 3) the knots get fed into FCL's SplineMotion, constructing a new spline

The main contention is on point 2 and its aftereffects. Multiplying the velocities causes the knots to get adjusted, resulting in incorrect splines. While the math might make sense to you, geometrically to me it just doesn't make sense.

[mxgrey]

I recommend you adjust your visualizer to work with arbitrary time ranges, and then we can revisit this concern.

[mxgrey]

Also keep in mind that the position values of the knots will depend on the time range of the spline. Those knot positions are easiest to calculate for time ranges that are strictly t=[0.0, 1.0], which is why the transformation is used in the first place.

[mxgrey]

If you want a geometric understanding of it, think about it this way:

Let's say you move from point A to point B while following some arc. You have some kind of velocity at the beginning of the arc and some other velocity at the end, and it takes you 10 seconds to get from point A to point B.

Now you want to follow the same exact arc, but you want to get from point A to point B in 1 second instead of 10 seconds. Every step of the way, you want to follow the same exact arc, you just want to do it in 1/10 of the time scale. For that to be possible, your velocity needs to be 10x as much every step of the way.

[ddengster]

Let's say you move from point A to point B while following some arc. You have some kind of velocity at the beginning of the arc and some other velocity at the end, and it takes you 10 seconds to get from point A to point B.

Would you expect the space covered to look different as you adjust the timings of the trajectory? What would the the geometry of the space covered look like? It seem really weird that changing a timing variable would do something like that.

The following examples and results illustrate this.

    const rmf_traffic::Time time = std::chrono::steady_clock::now();
    Eigen::Vector3d pos = Eigen::Vector3d(0, 0, 0);
    Eigen::Vector3d vel = Eigen::Vector3d(0, 0, 0);
    rmf_traffic::Trajectory t1;
    t1.insert(time, pos, vel);
    t1.insert(time + 1s, pos, vel);

    rmf_traffic::Trajectory t2;
    t2.insert(time, Eigen::Vector3d(-2, 2, 0), Eigen::Vector3d(0, -2, 0));
    t2.insert(time + 1s, Eigen::Vector3d(2, 2, 0), Eigen::Vector3d(0, 2, 0));

    CHECK(rmf_traffic::DetectConflict::between(profile_circle, t1, profile_circle_with_circle_offset, t2)); //collision

    const rmf_traffic::Time time = std::chrono::steady_clock::now();
    Eigen::Vector3d pos = Eigen::Vector3d(0, 0, 0);
    Eigen::Vector3d vel = Eigen::Vector3d(0, 0, 0);
    rmf_traffic::Trajectory t1;
    t1.insert(time, pos, vel);
    t1.insert(time + 10s, pos, vel);

    rmf_traffic::Trajectory t2;
    t2.insert(time, Eigen::Vector3d(-2, 2, 0), Eigen::Vector3d(0, -2, 0));
    t2.insert(time + 10s, Eigen::Vector3d(2, 2, 0), Eigen::Vector3d(0, 2, 0));

    CHECK(rmf_traffic::DetectConflict::between(profile_circle, t1, profile_circle_with_circle_offset, t2)); //no collision

[mxgrey]

Would you expect the space covered to look different as you adjust the timings of the trajectory? What would the the geometry of the space covered look like? It seem really weird that changing a timing variable would do something like that.

Yes, if you change the duration of a spline without changing its velocities, you may (and most likely will) change the space that it covers (an obvious exception would be a trivial stationary trajectory).

Try to imagine yourself running from Point A to Point B and then back to Point A. For your first run, you need to spend exactly 10 seconds running from A -> B -> A. For your second run, you need to spend exactly 1 second running from A -> B -> A. Can you imagine any plausible way to do that without running any faster? If you try to run at the same speed both times, then you won't be able to make it all the way to Point B before you have to return to Point A. That's what you're seeing in your tests where you're ending up with different arcs.

[ddengster]

Try to imagine yourself running from Point A to Point B and then back to Point A. For your first run, you need to spend exactly 10 seconds running from A -> B -> A. For your second run, you need to spend exactly 1 second running from A -> B -> A. Can you imagine any plausible way to do that without running any faster? If you try to run at the same speed both times, then you won't be able to make it all the way to Point B before you have to return to Point A. That's what you're seeing in your tests where you're ending up with different arcs.

To do the 1 second run, you would have to take the 'shorter path' since you can't adjust your speed. By this logic, if you want to do a hour you'd have to take the quite a detour of the spline motion. Do you not see a problem there?

[mxgrey]

If you want to cover the same path but spend an hour doing it, then you need to go much slower, which is why we're multiplying the velocities by the time scale when we transform to the unitary time scale.

If you want to spend an hour moving at the same velocities, then yes, the new spline you've created will take a very long detour compared to the spline with the shorter timescale.

[ddengster]

If you want to cover the same path but spend an hour doing it, then you need to go much slower, which is why we're multiplying the velocities by the time scale when we transform to the unitary time scale.

If you multiply velocities by the time scale and use it to reconstruct a new spline, the motion is different. Also if " you need to go much slower" weren't we saying that we weren't going to change velocities a few posts earlier?

If you want to spend an hour moving at the same velocities, then yes, the new spline you've created will take a very long detour compared to the spline with the shorter timescale.

If I'm writing trajectory code, given how the trajectory api is done I'd expect the motion of a cubic hermite spline without compensating for the duration. Otherwise given the current methods the planner could test for splines completely outside of the area depending on the duration given for the trajectory.

There's also a contradiction in that if geometry being changed, it is not a remapping as mentioned.

[mxgrey]

It seems like you're missing the detail that the velocities and the time scale need to be transformed simultaneously and in proportion to each other in order to correctly remap the spline from one time scale to another.

[ddengster]

It seems like you're missing the detail that the velocities and the time scale need to be transformed simultaneously and in proportion to each other in order to correctly remap the spline from one time scale to another.

I'm concerned about the expected and resultant 'collision geometry' being used. Considering that in the examples that there is one completely stationary robot and one moving robot we can completely discard the notion of velocity and corner ourselves into just looking at the geometry.

Then the debate becomes why we are using the newly made, adjusted splines that could go out of bounds when the intention submitted is the motion of a cubic hermite spline.

[mxgrey]

Then the debate becomes why we are using the newly made, adjusted splines that could go out of bounds when the intention submitted is the motion of a cubic hermite spline.

The whole point that I'm making is that when we transform the velocities and the time scales proportionally to each other, we get an equivalent (meaning it follows the same arc) spline transformed to the new time scale. If you adjust the time scale but not the velocity, then you get a non-equivalent (meaning it follows a different arc) spline.

[ddengster]

The whole point that I'm making is that when we transform the velocities and the time scales proportionally to each other, we get an equivalent (meaning it follows the same arc) spline transformed to the new time scale.

I believe there is a huge flaw in this reasoning. If you happen to have any material that describes this, please share it. The way we're plugging values into FCL tells me that we're assuming a spline in the global space and checking for collisions in that space.

[mxgrey]

Here's a video to illustrate the reason we need to scale both time and velocity together when transforming the time coordinate to a unary scale:

Note that the trajectories are intentionally offset from each other when displayed so both can always be seen at the same time. Both trajectories have the same exact path through space, but since they operate on different time scales they also need to move at different velocities. If you do not change the velocities proportionally to the change in time scale when you transform the spline, then you will get distortions in the spline's path, like this:

Here are the same two trajectories, except the green trajectory's endpoint velocities are not scaled proportionally to its time scale. Instead it has 1/20th the time scale of the red trajectory but the velocities at its endpoints are equivalent to the red trajectory:

I'll be closing this issue now, since there is not a bug.