# How to run Matlab Trajectory Planning script?

**URL:** <http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247>\
**Category:** OpenCN Framework\
**Created:** [December 2, 2021, 12:50pm UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247 "2021-12-02T12:50:02Z")\
**Posts on this page:** 20\
**Page:** 1

<div class="post-metadata">

**Author:** ![yakunix](http://discourse.heig-vd.ch/letter_avatar_proxy/v4/letter/y/bb73d2/32.png) [@yakunix](http://discourse.heig-vd.ch/u/yakunix)\
**Post date:** [December 2, 2021, 12:50pm UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/1 "2021-12-02T12:50:02Z")

</div>

Hi OpenCN developer,  
After reading the documentation I understand that algorithm is implemented with Matlab and then converted to C++ code for real-time computation. But I’m not sure which Matlab script should I start from to run a trajectory planning example? And which script should I run to generate code for different platforms? Is there a workflow documentation for it or Could you give me some advice?

---

<div class="post-metadata">

**Author:** ![Raoul](http://discourse.heig-vd.ch/user_avatar/discourse.heig-vd.ch/raoul/32/21_2.png) [@Raoul](http://discourse.heig-vd.ch/u/Raoul)\
**Post date:** [December 2, 2021, 3:40pm UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/2 "2021-12-02T15:40:22Z")

</div>

Hi,

one possibility is to run the script UnitTests.m in

matlab/common/UnitTests.m

The basic lines are :  
cfg = FeedoptDefaultConfig;  
cfg.source = ‘your\_file.ngc’;  
ctx = InitFeedoptPlan(cfg);  
ctx = FeedoptPlanRun(ctx);

The scipt for generating C code is  
generate\_c.m

BUT : for the moment only possible with Matlab R2020a  
We are working on a solution

best regards,

Raoul

---

<div class="post-metadata">

**Author:** ![yakunix](http://discourse.heig-vd.ch/letter_avatar_proxy/v4/letter/y/bb73d2/32.png) [@yakunix](http://discourse.heig-vd.ch/u/yakunix)\
**Post date:** [December 3, 2021, 7:56am UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/3 "2021-12-03T07:56:19Z")

</div>

Hi Raoul,

I found a script Validate\_OpenCN.m. And after executing it and plot the planned results, I found some oscillations in the velocity file at the end or start of the ramp stage. Ideally it should be S curve? Will the fluctuation have an influence on the machining process?

[![2021-12-03-3-35-10](https://discourse.heig-vd.ch/uploads/default/original/1X/f72428c927652447a0788306d142aebf2d6e50e3.png)](https://ibb.co/WKZxr2v)

[![2021-12-03-3-35-56](https://discourse.heig-vd.ch/uploads/default/original/1X/410862c5aeeb03331997d44b36483ac1b4d875e7.png)](https://ibb.co/rH09vmp)

From the documentation, the planning is optimization based. Do you have any reference about the planning method? Thanks!

---

<div class="post-metadata">

**Author:** ![yakunix](http://discourse.heig-vd.ch/letter_avatar_proxy/v4/letter/y/bb73d2/32.png) [@yakunix](http://discourse.heig-vd.ch/u/yakunix)\
**Post date:** [December 3, 2021, 8:30am UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/4 "2021-12-03T08:30:48Z")

</div>

Here is the velocity and position plot of the file **spline\_osc.ngc** :

[![2021-12-03-4-24-49](https://discourse.heig-vd.ch/uploads/default/original/1X/e94cce045f9a11a3c7ca3bc33f044030c7eb597f.png)](https://ibb.co/TMjYcY8)

The velocity also vibrates. So is this a normal behavior of the current TP algorithm? I think ideally the velocity profile should be a 7-phase S curve.

---

<div class="post-metadata">

**Author:** ![Raoul](http://discourse.heig-vd.ch/user_avatar/discourse.heig-vd.ch/raoul/32/21_2.png) [@Raoul](http://discourse.heig-vd.ch/u/Raoul)\
**Post date:** [December 3, 2021, 9:42am UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/5 "2021-12-03T09:42:10Z")

</div>

There are many things to say :  
The oscillations are of course unwanted.  
They progressively disappear if the global parameters  
NBreak and NDiscr are increased.  
NBreak is the number of breakpoints for the spline q(u) used in Feedrate planning. NDiscr is the number of discretization points for the constraints.  
Generally NDiscr should be roughly 2 times NBreak as a rule of thumb.  
I believe the default values are NDiscr = 140 and NBreak = 60.

Spline support for compressing multiple small G01 segments is not yet fully operational in the master branch. It can be disactivated using parameter “Compressing” (file matlab/common/c\_planner\_v5/FeedoptDefaultConfig.m).

Improvements on the compressing feature are still in a separate branch which will be merged to the master soon.

---

<div class="post-metadata">

**Author:** ![Raoul](http://discourse.heig-vd.ch/user_avatar/discourse.heig-vd.ch/raoul/32/21_2.png) [@Raoul](http://discourse.heig-vd.ch/u/Raoul)\
**Post date:** [December 3, 2021, 9:53am UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/6 "2021-12-03T09:53:43Z")

</div>

For a long straight line segment with stand still at both ends the optimal solution is of course a 7 phase trajectory where the accelaration has a trapezoidal shape :  
rising, constant, falling, zero, falling, constant, rising  
This was of course one of our first tests 🙂

Please find enclosed a doc that also helps understanding the path planning.  
[PROCIR\_HPC\_2020\_OpenCN\_v6\_revised.pdf](http://discourse.heig-vd.ch/uploads/short-url/dMAnqwXH900zyGbGiPk3tht3Xej.pdf) (3.1 MB)

---

<div class="post-metadata">

**Author:** ![yakunix](http://discourse.heig-vd.ch/letter_avatar_proxy/v4/letter/y/bb73d2/32.png) [@yakunix](http://discourse.heig-vd.ch/u/yakunix)\
**Post date:** [December 4, 2021, 11:38am UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/7 "2021-12-04T11:38:18Z")

</div>

Thank you Raoul! The paper is quite explainable. Only a few details I wish to understand further.

1. Why the zero speed should be handled separately and the uk calculated in the following equation instead of Taylor series expansion? And what value should the constant pseudo jerk take?  
 ![](https://latex.codecogs.com/svg.latex?%5Cinline&space;%5CLARGE&space;%7B%5Ccolor%7BYellow%7D&space;u_%7Bk%7D=%5Cfrac%7B1%7D%7B6%7Dj_%7Bps%7D(k%5CDelta&space;t)%5E%7B3%7D%7D "\LARGE {\color{Yellow} u\_{k}=\frac{1}{6}j\_{ps}(k\Delta t)^{3}}")
2. From Table 1, the number of knots for q(u) Nk is 10 but did not give the knot vector value {u0, u1, …, uk} and what is meaning of grid points for inequalities Ng?

 ![image](https://discourse.heig-vd.ch/uploads/default/original/1X/3ef2391eac6daf8ff952ede7e0b01c91bce3e2f1.png)

1. Why is too short or too long curves not preferred and they should be homogenized?

---

<div class="post-metadata">

**Author:** ![Raoul](http://discourse.heig-vd.ch/user_avatar/discourse.heig-vd.ch/raoul/32/21_2.png) [@Raoul](http://discourse.heig-vd.ch/u/Raoul)\
**Post date:** [December 4, 2021, 12:29pm UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/8 "2021-12-04T12:29:41Z")

</div>

Hi yakunix,

1. There is a singularity at zero speed : if you use the Taylor series expansion you will stay at zero speed. Therefore the trick for using a constant pseudo jerk at the beginning. We implemented an algorithm for adapting the pseudo jerk in this phase of motion. We start with an initial value. If the constraints are violated we multiply the pseudo jerk by a factor \< 1. This is repeated in a while loop until we find a pseudo jerk that does not violate the constraints. Even if it is not time optimal, this does not matter since it is only an extremely small part of the overall trajectory.

2. There are multiple knots at u=0 and u = 1. Refer to standard textbooks on B-Splines, eg. de Boor. Of course Nk (called NBreak in the code) is related to the number of basis functions of q(u).  
The meaning of Ng (called NDiscr in the code) is the following : The constraints have to be respected for _all_ real value of u in the interval [0, 1]. This means for infinitely many values. In order to convert it to a standard finite dimensional optimization problem, we only formulate the constraints on a finite grid of u values between 0 and 1. There is a technique called “spline relaxation” which can avoid this under certain conditions. I will explore this in a next release. I can send you publications on this topic.

3. It is important that the optimization problems _all_ have the same structure, same number of unknowns, same number of constraints, same sparsity structure. If NDiscr is fixed, the constraints for a long curve piece would be too rough. Discretizing extremely small curve pieces with the same NDiscr makes no sense since computation time will be too high. Homogenizing the length of curve pieces also helps to improving the conditioning of the optimization problems.

---

<div class="post-metadata">

**Author:** ![yakunix](http://discourse.heig-vd.ch/letter_avatar_proxy/v4/letter/y/bb73d2/32.png) [@yakunix](http://discourse.heig-vd.ch/u/yakunix)\
**Post date:** [December 9, 2021, 10:02am UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/9 "2021-12-09T10:02:15Z")

</div>

Hi Raoul,

Thank you so much for the detailed information.

I spent some time reading the code and got some new doubts:

1.Could the zero speed singularity be handled by the following relation between _ **t** _ and _ **u** _?

![](https://latex.codecogs.com/svg.latex?%5Clarge&space;%7B%5Ccolor%7BYellow%7D&space;t&space;=&space;%5Cint_%7B0%7D%5E%7Buk%7D%5Cfrac%7B1%7D%7B%5Cdot%7Bu%7D%7Ddu%7D "\large {\color{Yellow} t = \int\_{0}^{u}\frac{1}{\dot{u}}du}")

The question becomes: Given the time t, find the uk that satisfy the relation. We may use the bi-section method to search for the uk while for the integration, we may use Simpson or Gaussian integration method to approximate the result. Once we have got the solution uk for the first time tick (1e-4 in the code), then we are away from the singularity and then Taylor series expansion could be used.

2.The knot vector for q(u) is generated using the _sinspace_ function. Is it a hypothesis for the optimization problem and what is motivation for using this type of knot vector?

3.In classic text book, the relation between number of control point _ncoeff_ (n+1), number of knot vector _nbreak_ (m+1) and degree of spline _degree_(p) is: [_m_ = _n_ + _p_ + 1](https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/B-spline/bspline-curve.html), hence, _ncoeff_ = _nbreak_ - _degree_ - 1. But in the code (bspline\_create.m line 3), it is _ncoeff_ = _nbreak_ + _degree_ - 2. I am not sure which one is right? Also, in the code, the spline degree is 4 rather than 3 in the paper, does it give a better result because typically higher degree would cause oscillations in the spline derivatives.

4.What are the meaning of the a\_param, b\_param, sp\_index in the CurvStruct?

5.Could we simultaneously process the geometry smoothing and velocity planning? It seems as long as there are enough smoothed geometry in the queue, we can start to do the velocity planning. And could we start the motion once the first block in the horizon finished optimization?

---

<div class="post-metadata">

**Author:** ![Raoul](http://discourse.heig-vd.ch/user_avatar/discourse.heig-vd.ch/raoul/32/21_2.png) [@Raoul](http://discourse.heig-vd.ch/u/Raoul)\
**Post date:** [December 9, 2021, 11:10am UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/10 "2021-12-09T11:10:36Z")

</div>

Hi Yakunix,

I’m happy to have somebody asking good questions !  
Welcome on board !

1. At zero speed you have a singularity in the integrand.  
In the paper of Verscheure [VDS+09]  
[https://mecatronyx.gitlab.io/opencnc/opencn/CNC\_Path\_Planning\_Algorithms/References/References.html](https://mecatronyx.gitlab.io/opencnc/opencn/CNC_Path_Planning_Algorithms/References/References.html)  
this singularity can be analytically solved if the unknown function q(u) is piecewise linear. However in order to have limited jerk this is not the case.  
We spent a lot of time on the question how to handle this singularity, and the only solution we found is the idea to start with a constant pseude jerk.

2. The choice of the knot vector can be either equidistant (linspace) or a higher density at the start and the end points. The function sinspace generates this _non_ equidistant knot distribution. The idea behind was that changes in acceleration/deceleration are more likely to happen at the beginning and the end of each curve piece.  
And the discretization scheme is more accurate if there is a higher density of knots.  
Maybe it was not necessary.

3. We played around with the spline degree; higher than 4 does not give any improvement. I’ll think about the other question.

4. The compressing yields a spline in R^3 which can be quite long.  
The “splitting” homogenizes the length of of the curve pieces.  
In order sto split, we introduce an affine function a\_param\*u + b\_param on curve parameter u. If a\_param = 1 and p\_param = 0 you have the entire spline.  
This means that _several_ curve pieces can share the _same_ spline coefficients.  
sp\_index is a “pointer” to the Spline coefficients which are _not_ stored in the CurvStructs.

5. YES, this is the idea. In fact the feedrate planning needs the most computation time.  
Interpreting the .ngc file is extremely fast. The geometrical operations are also very fast.  
No need to have parallel threads. However the feedrate optimization is really a parallel thread.

best regards,

Raoul

---

<div class="post-metadata">

**Author:** ![yakunix](http://discourse.heig-vd.ch/letter_avatar_proxy/v4/letter/y/bb73d2/32.png) [@yakunix](http://discourse.heig-vd.ch/u/yakunix)\
**Post date:** [December 9, 2021, 12:34pm UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/11 "2021-12-09T12:34:01Z")

</div>

Hi Raoul, thanks for your reply. It’s really helpful.  
I think the variable _SplineDegree_ in FeedoptDefaultConfig should be renamed to _SplineOrder_, because typically order = degree + 1. Or we can set _SplineDegree_ = 3, and pass _SplineDegree_ + 1 to gsl function.

---

<div class="post-metadata">

**Author:** ![yakunix](http://discourse.heig-vd.ch/letter_avatar_proxy/v4/letter/y/bb73d2/32.png) [@yakunix](http://discourse.heig-vd.ch/u/yakunix)\
**Post date:** [December 9, 2021, 1:03pm UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/12 "2021-12-09T13:03:04Z")

</div>

One more question… 😅

I noticed the equality constraints on the acceleration is enforced in its tangential component (with the projection operation). In this case, would the overall acceleration still be continuous considering the normal components?

---

<div class="post-metadata">

**Author:** ![Raoul](http://discourse.heig-vd.ch/user_avatar/discourse.heig-vd.ch/raoul/32/21_2.png) [@Raoul](http://discourse.heig-vd.ch/u/Raoul)\
**Post date:** [December 9, 2021, 1:59pm UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/13 "2021-12-09T13:59:06Z")

</div>

It is sufficient to consider the equality constraint to enforce continuity of the _tangential_ component, since the curvature at each junction point is continuous (G2 continuity).  
Both components of the acceleration, normal and tangential will automaticall be continuous.

---

<div class="post-metadata">

**Author:** ![yakunix](http://discourse.heig-vd.ch/letter_avatar_proxy/v4/letter/y/bb73d2/32.png) [@yakunix](http://discourse.heig-vd.ch/u/yakunix)\
**Post date:** [December 10, 2021, 12:10am UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/14 "2021-12-10T00:10:52Z")

</div>

That is Cool. Thanks!

---

<div class="post-metadata">

**Author:** ![yakunix](http://discourse.heig-vd.ch/letter_avatar_proxy/v4/letter/y/bb73d2/32.png) [@yakunix](http://discourse.heig-vd.ch/u/yakunix)\
**Post date:** [January 7, 2022, 7:11am UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/15 "2022-01-07T07:11:55Z")

</div>

Hi Raoul,  
In the HPC paper, the remaining time is approximated by the following equation:

![image](https://discourse.heig-vd.ch/uploads/default/original/1X/7fadd3b3a2b6f21f0919ba4c482c12a7123b78ae.png)

I’m wondering if this strategy could also be used for singularity handling. At the start of zero velocity, we can assume the q(u) to be linear, so we have the relation:

![](https://latex.codecogs.com/svg.image?%5Ccolor%7BYellow%7D(u_%7Bk%7D&space;-&space;0)/%5Cleft&space;%5B&space;(%5Csqrt%7Bq(u_%7Bk%7D)%7D&space;-&space;0)/2&space;%5Cright&space;%5D=t_%7Bk%7D "(u\_{k} - 0)/\left [(\sqrt{q(u\_{k})} - 0)/2 \right]=t\_{k}")

Then we have:  
 ![](https://latex.codecogs.com/svg.image?%5Ccolor%7BYellow%7D%5Cfrac%7B4*%7Bu_%7Bk%7D%7D%5E%7B2%7D%7D%7Bq(u_%7Bk%7D)%7D=%7Bt_%7Bk%7D%7D%5E%7B2%7D "\frac{4\*{u\_{k}}^{2}}{q(u\_{k})}={t\_{k}}^{2}")  
As q(uk) is a cubic spline and because q(0) is zero at start, we have q(u) to be the following format:  
 ![](https://latex.codecogs.com/svg.image?%5Ccolor%7BYellow%7Da*%7Bu_%7Bk%7D%7D%5E3+b*%7Bu_%7Bk%7D%7D%5E2+c*%7Bu_%7Bk%7D%7D "a\*{u\_{k}}^3+b\*{u\_{k}}^2+c\*{u\_{k}}")  
Substitute it into the denominator, we have:

![](https://latex.codecogs.com/svg.image?%5Ccolor%7BYellow%7D%5Cfrac%7B4*%7Bu_%7Bk%7D%7D%5E2%7D%7Ba*%7Bu_%7Bk%7D%7D%5E3+b*%7Bu_%7Bk%7D%7D%5E2+c*%7Bu_%7Bk%7D%7D%7D=%7Bt_%7Bk%7D%7D%5E2 "\frac{4\*{u\_{k}}^2}{a\*{u\_{k}}^3+b\*{u\_{k}}^2+c\*{u\_{k}}}={t\_{k}}^2")

This uk has a closed form of solution to be:

 ![image](https://discourse.heig-vd.ch/uploads/default/original/1X/8b673b1d352a6c7cd02bf3f48ea0e3046e52a0ca.png)

And we can choose the minimum positive solution.

Do you think the derivation is correct or did I mistake something?

Thanks!

---

<div class="post-metadata">

**Author:** ![Raoul](http://discourse.heig-vd.ch/user_avatar/discourse.heig-vd.ch/raoul/32/21_2.png) [@Raoul](http://discourse.heig-vd.ch/u/Raoul)\
**Post date:** [January 7, 2022, 7:53am UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/16 "2022-01-07T07:53:37Z")

</div>

hmmmm …  
q(u) linear at the beginning will not work since the acceleration would jump which violates the jerk constraint.  
In my memory we came to the conclusion that a cubic q(u) or even an arbitrary polynomial q(u) will not work in the beginning. There must be fractional powers in q(u).  
It’s a long time ago, and I don’t remember all reasonings.

---

<div class="post-metadata">

**Author:** ![yakunix](http://discourse.heig-vd.ch/letter_avatar_proxy/v4/letter/y/bb73d2/32.png) [@yakunix](http://discourse.heig-vd.ch/u/yakunix)\
**Post date:** [January 7, 2022, 8:07am UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/17 "2022-01-07T08:07:59Z")

</div>

Okay… Sounds strange.

But when we formulate the problem, the constraint includes the initial boundary condition (v0 = 0, a0 = 0), right? In this case, the velocity and acceleration should be zero if we adopted the optimization results. Do you mean even if we uses the optimized q(u), the acceleration will still have a jump at the first cycle time? This will be contradictory since the solver thinks it has found a solution that will not violate the jerk constraint. 😅

We can use very small tk like 1e-8 to solve the uk, and the spline q(u) will be almost linear at the infinite-small range . Once we have the first solution, then we are safe to use Taylor series expansion to get the remaining uk.

---

<div class="post-metadata">

**Author:** ![yakunix](http://discourse.heig-vd.ch/letter_avatar_proxy/v4/letter/y/bb73d2/32.png) [@yakunix](http://discourse.heig-vd.ch/u/yakunix)\
**Post date:** [January 7, 2022, 8:28am UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/18 "2022-01-07T08:28:50Z")

</div>

In the paper _Linear programming feedrate optimization - Adaptive path sampling and feedrate override_, it is mentioned that if we use the Greville abscissae as the evaluation sampling points, the optimization will be more stable since it results in a sparse M matrix. Current code uses a sinewave function to discretize the path. Maybe it is worth having a try.

 ![image](https://discourse.heig-vd.ch/uploads/default/original/1X/866e3afb9861331b723a209563665be709a1895a.png)

---

<div class="post-metadata">

**Author:** ![Raoul](http://discourse.heig-vd.ch/user_avatar/discourse.heig-vd.ch/raoul/32/21_2.png) [@Raoul](http://discourse.heig-vd.ch/u/Raoul)\
**Post date:** [January 7, 2022, 8:36am UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/19 "2022-01-07T08:36:23Z")

</div>

thanks for your hint, that might be worth trying.  
We started with an uniform knot distribution.  
But acceleration/deceleration is likely to happen at the beginning and at the end of a curve piece. Therefore the “knot density” should be higher at the beginning and at the end. This leads us to use “sinspace” instead of linspace.  
I never heard about Greville abscissae; I will have a look and come back to you later.

---

<div class="post-metadata">

**Author:** ![Raoul](http://discourse.heig-vd.ch/user_avatar/discourse.heig-vd.ch/raoul/32/21_2.png) [@Raoul](http://discourse.heig-vd.ch/u/Raoul)\
**Post date:** [January 7, 2022, 8:38am UTC](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247/20 "2022-01-07T08:38:10Z")

</div>

The wanted boundary conditions are indeed v0 = 0 and a0 = 0.  
I will check with my mathematician friend Philippe Blanc next week and come back to you.

[Next page](http://discourse.heig-vd.ch/t/how-to-run-matlab-trajectory-planning-script/247.md?page=2)
