scurve trajectory planner

@Aciera, you are right it was probably to early this morning. I was thinking of the first derivative of the tangent vector (v_l) which makes it the second derivative. Sorry for the confusion.

But regardless I guess we can't simply take 1/R for the curvature because we need the components of it in x, y and z and these depend on the orientation of the arc etc.

Hi,

Thank's again Arciera & Fabian.


I was able to figur out Eq.29 & Eq.30.  

This part includes the input of a line or arc from 3d space.
It includes calculating unit vector's.
It includes a arc interpolation given a arc circumfence lenght s.
It then includes calculating the first (theta), second (kappa) and third derivate (sharpness) given the lenght s.

For Eq.29 & Eq.30 it includes a proof function, given a example arc as input. This way we can verify the output.
There is terminal output visible for the proof functions, pasted as comment text.

Maybe you guy's can also verify the output.

Based on this work, i think best to restart with Eq.39 again.
 

Hi Arciera,

I will make a repository for this. Then testing is easy.

Just found a exception. At page 5, where va1 & va2 are calculated.

This are arc related vectors, 90 degrees in between. And also need the arc normal.

However the proof function uses a example arc where the arc start, arc center, arc end are colinear.
The result is a arc normal vector [0,0,0] wich is not good.

So i think we have to ship the arc's containing the normal unit vector. Just as we do inside lcnc's
trajectory planner.

Edit : Now i shipped the arc normal into the funtions and things work for eq29, eq30.

Link to repository

Edit:
Now at the point :

eq29_proof(); -> interpolate arc
eq30_proof(); -> calculate derivates.
eq31_v1_proof(); -> calculate vector v1 & v2
eq31_v2_proof();
eq35_proof(); -> forward transformation 2 segments (did just one test, it does rotate the segments.).

Hi Grotius,

I guess you calculated va1 directly as the vector between centerpoint P_c and Startpoint P_a0 and va2 by first calculating n via the cross product between va1 and the vector between P_c and end of the arc P_as?
In the colinear case this will not work as the circle is not clearly defined in this situation, so it is required to get the additional info from LCNC in which plane this arc is supposed to be so your solution is the way to go.

@Fiabian,

Thank you for checking this.

Question 1:
I updated the related code for eq 39. link    

Page 7, eq39.
Here the abstract states that : "s0, se are parameters at the start, end point of clothoid."

When i think about that. It's meaning or pointing to the fillet start, end point for the clothoid spline.
s0, se are in my opinion waypoints on segment 1 & 2, where the spline should start at s0, and end at se.
Can you confirm this?

Question 2:
Page 7, eq35.

Here the 2 segment transformation is done, It multiplies the segment points with the matrix T. Everything ok.

Then for the derivates is proposes to also multiply the derivates with the matrix T.
I have my questions about the workflow here.

1. We start with 2 segments in 3d space.
2. We apply transformation. The transformation uses the workflow:  eq35 -> eq33 -> eq33->eq30_theta.
3. For the transformation only one time we use eq30_theta to get the first derivate theta. Note: no cpu consuming task.
4. Transformation is done.

Given steps 1-4, only one time the first derivate is calculated. 
We did not calculate all derivates when segments where at initial 3d space position.

So eq35 to multiply all derivates by the matrix T is rendundant.
We can better calculate all the derivates when segments are in transformed position, only then we need them.

Question 3:
For the inputs of eq39.

If we look at the first line   :  Theta1.0 = asin ( ztraj ( so ) )

The first derivate theta for a point P consists out 3 values, x,y,z.
Then we used the transformed point P here.
Then for ztraj, we used the first derivate theta, the z value.

As in question 1. I think s0 & se are fillet start, end points to use here.

Question 4:
Until now, no Fresnels where required?

@Grotius

Question 1:
yes s0 and se are the end points. The first clothoid of the spline P(s) starts at s0 and the last (4th) clothoid of the spline ends at se.
P_traj.(s0) = P(0) and P(s4) = P_traj.(se)
P(s) is the clothoid spline and P_traj.(s) are the curves at the beginning and end of the spline.

Question 2:
That's correct. You can either calculate first all the derivatives and transform them afterwards or you can first transform v_a1 and v_a2 and then calculate the derivatives.

Question 3:
Exactly, this is one of the boundary conditions. it is the z component of the first derivative in the starting point s_0 in the transformed coordinate system.

Question 4:
Fresnel Integrals show only up in equation 44. Until now we did not need them.

Hi Fabian,

+1 Yes calculating derivates after transforming is more efficient.

Up to 43 is done. Except eq41.
eq39 theta
eq40 kappa
eq42 sharpness
eq43
eq45

Now starting the first 3 of eq44. G3 continuity. Hmm.
I didn't know the big SUM sign. It seems a for loop. Starting at h=1 up to h=4.
That being clearified. We can try to code this.