[Home]Simple Tp Notes

LinuxCNCKnowledgeBase | RecentChanges | PageIndex | Preferences | LinuxCNC.org


1. the key data structures and functions
1.1. TP_STRUCT (tp.h and tp.c)
1.1.1. TP_STRUCT functions
1.2. TC_QUEUE_STRUCT and TC_STRUCT (tc.h / tc.c)
1.2.1. TC_QUEUE_STRUCT functions
2. How trajectory planning works
3. Trajectory planning algorithms
3.1. common tpRunCycle tasks which all TPs do
3.2. simple_tp
3.2.1. simple_tp math
3.2.2. simple_tp math (update 2011 May)
3.3. The EMC 2.1.0 TP
simple_tp is a tag [announced on emc-developers] by cradek on 3 Jan 2006 which implements a simple 'exact stop' trajectory controller. The source is documented and meant to be easy to read/understand. The files are in emc2/src/emc/kinematics:
 tp.h and tp.c -- has the all important TP_STRUCT and functions for adding lines/circles to the motion queue 
                  + the 'interpolator' function tpRunCycle?

 tc.h and tc.c -- Implement a fairly simple motion/segment queue with functions for adding, deleting and accessing segments.

 mmxavg.c and mmxavg.h -- 

 cubic.h and  cubic.c -- calculate coefficients of cubic splines 
   (is this for position vs. time trajectory generation or geometrical belnding ??) (methinks for position vs. time...)

 kinematics.h and trivkins.c -- implemets trivial kinematics i.e. each joint (0..5) corrensponds directly to each axis (XYZABC)

Here are some notes on how I (awallin) think it all works. (started this on 25Jul2006 - hopefully I or someone else can continue until the trajectory controller is well documented/understood)

1. the key data structures and functions

1.1. TP_STRUCT (tp.h and tp.c)

tp.h and tp.c define the TP_STRUCT and associated functions. TP_STRUCT holds information about the current state of the machine and the trajectory controller. I think that in EMC there is never more than one of these TP_STRUCTs created.

The content of TP_STRUCT seems to fall roughly into three different categories.

1.1.1. TP_STRUCT functions

tp.c implements a number of functions that operate on this structure. These can roughly be grouped as follows:

AddLine? and AddCircle? are pretty straightforward functions that add the require segment to the motion queue. *FIXME: there are some calls to pmCircleInit? and pmLineInit? which I do not understand...

Most, if not all, of the 'intelligence' of the trajectory planning lies in tpRunCycle?(). This function is called every TRAJ_PERIOD and its task is to update the tp->currentPos (and tp->currentvel) to reflect the desired motion of the machine. It does this by going through the segments in the motion queue one by one. It is here that the motion mode (G61.1/G61/G64) makes a difference (blending is either applied or not).

Looking at cubic.h it seems that currently the position vs. time trajectories are cubic splines.

1.2. TC_QUEUE_STRUCT and TC_STRUCT (tc.h / tc.c)

tc.h and tc.c define the motion queue TC_QUEUE_STRUCT, the motion structure TC_STRUCT, and associated functions that operate on these.

TC_QUEUE_STRUCT is a queue of TC_STRUCT elements. TC_QUEUE_STRUCT has:

1.2.1. TC_QUEUE_STRUCT functions

there are functions to tcqCreate, tcqDelete, and tcqInitialize the queue.

tcqPut and tcqRemove functions either add or delete TC_STRUCTs to/from the queue.

tcqLen returns _len, tcqItem returns the nth TC_STRUCT, tcqFull indicates wheter the queue is full.

TC_STRUCT defines each segment of motion (either a linear or a circular/helix move). It contains fields for

most functions that operate on TC_STRUCT seem to reside in tp.c and not tc.c. one function in tc.c does operate on TC_STRUCT: tcGetPos?() takes a TC_STRUCT as the only argument and returns an EmcPose? somehow related to the argument.

2. How trajectory planning works

could it be this simple?:
  1. interpreter calls tpAddLine?/tpAddCircle? according to what it gets as input (MDI, G-code)
  2. tpRunCycle? is called every TRAJ_PERIOD. It updates currentPos
  3. currentPos is fed more or less directly to the axis.X.motor-pos-cmd HAL pins ?

3. Trajectory planning algorithms

EMC employs a trapezoidal velocity profile generator. The plot of the velocity vs. time has either constant velocity regions (cruise regions) or regions with constant acceleration/decceleration. The position is the integral of the velocity vs. time plot and thus consists of linearly increasing or decreasing regions corresponding to cruise phases in velocity, and parabolic regions which correspond to the acc/decceleration velocity regions.

The acceleration vs. time is piecewise continuous - but the jerk is infinite (where the acceleration changes or the velocity vs. time has a sharp 'corner'). There are jerk-limited or double-jerk-limited trajectory planners described in the litterature but they have not been implemented in EMC (yet?).

Something about time-optimality here ? (jerk or djerk planners are not time-optimal, the trapezoidal planner is ?)

3.1. common tpRunCycle? tasks which all TPs do

3.2. simple_tp

This mode goes through the segments in the queue and executes them in order with zero start velocity and zero end velocity (G61.1 Exact stop mode).

If maximum velocity is ever reached it is maintained (cruise phase) until decceleration (at the maximum allowed decceleration) is begun. Thus there are two types of moves:

  1. If there is a cruise phase
    accelerate up to desired feedrate
    cruise phase
    deccelerate down to zero feed
  2. If there is no cruise phase
    accelerate until we are half way
    deccelerate down to zero during the other half

Here's how it's implemented in practice:


Here is a picture of how it works. The picture shows a segment of length 10 (i.e. target=10). The upper picture shows a position vs. time graph where we see how the machine is moved to X=10 during about 5000 trajectory cycles (each 0.001 s long).

The lower graph shows how the trajectory planner works. A suggested velocity is first calculated (green). This is a velocity that is as high as possible, but low enough so that it is possible to slow down to v=0 using the maximum allowed acceleration, while exactly reaching the target.

The suggestion (green), is then subject to two constraints, max velocity and max acceleration. First the max velocity is checked and adjusted if neccessary (this results in the red dots above), then maximum acceleration is checked and if this constraint is violated, a new velocity is calculated (shown in cyan).

The resulting velocity vs. time trajectory is shown as blue circles. If you look closely you can distinguish the colors: first, during the acceleration phase, we have cyan dots meaning that the acceleration clamp is active, second, during the 'cruise' phase, we have red dors meaning that the velocity clamp is active, and third, during the decceleration phase the suggested velocity is accepted as such and we have green dots.

This is the output of a quick-and-dirty implementation of simple_tp in matlab. There are still some problems with roundoff errors. (the matlab 'equivalent' of tpRunCycle?: upload:runcycle.m ) (a test script wich plots the above figure: upload:test.m )

3.2.1. simple_tp math

Looking at the picture above, the task is, for each call to tpRunCycle?, to calculate the suggested velocity (green dots). We then first apply the velocity clamp and then the acceleration clamp and adjust the suggested velocity as appropriate.

The suggested velocity should be as high as possible, but it should still be possible to exactly complete the move while deccelerating at maximum deccelration from the suggested velocity down to zero.


T = target, the position we want to reach
P = progress, our current position
v_s = the new suggested velocity (see pic)
t_m = the total time of the move (see pic)
a_m = the maximum allowed acceleration/decceleration

the area under the triangle is:
T-P = 1/2 * v_s * t_m
on the other hand v_s is (since when t_m seconds have passed we need to have v=0)
v_s = a_m * t_m
so we get
T-P = 1/2 * a_m * t_m * t_m
t_m = sqrt( 2 * (T-P) / a_m )
and that gives us the suggested velocity:
v_s = a_m * t_m = a_m * sqrt( 2 * (T-P) / a_m )

This is the math for the continuous time case. In the actual code there are correction terms which are required due to the discrete time sampling... these should be described here.

3.2.2. simple_tp math (update 2011 May)

See also blog post: http://www.anderswallin.net/2011/05/emc2-tpruncycle-revisited/

The above diagrams and math are for a continuous-time case and does not take into account the current velocity or the cycle time.

When we take into account the current velocity we are travelling at, as well as the cycle time we get slightly different picture:


T = target, the position we want to reach
P = progress, our current position
v_s = the new suggested velocity (see pic)
t_m = the total time of the move (see pic)
a_m = the maximum allowed acceleration/decceleration
v_c = the current velocity
t_s = the sampling/cycle time

The reasoning/derivation is exactly the same as above. We want the total travel to equal (T-P) and we calculate the total travel as the area under the velocity-curve. The area now splits into the green and red triangles above (math notation is Latex-ish):

T-P = {1 \over 2} t_s v_c + {1 \over 2} t_m v_s

on the other hand we know that v_s = a_m (t_m - t_s) which we can insert above to get:

T-P = {1 \over 2} t_s v_c + {1 \over 2} t_m a_m (t_m - t_s)

This is now a quadratic equation in t_m and we can solve for t_m using the quadratic formula to get:

t_m = {1 \over 2}t_s + \sqrt{ {t_s^2 \over 4} - {t_s v_c - 2(T-P) \over a_m} }

which we insert into the second formula to get our final answer for v_s:

v_s = a_m (t_m - t_s) = -{1 \over 2 }a_m t_s + a_m \sqrt{ {t_s^2 \over 4} - {t_s v_c - 2(T-P) \over a_m} }

This can be seen to be equivalent to the following lines of C-code (from void tcRunCycle?(TP_STRUCT *tp, TC_STRUCT *tc, double *v, int *on_final_decel)):

 discr = 0.5 * tc->cycle_time * tc->currentvel - (tc->target - tc->progress);
 discr = 0.25 * pmSq(tc->cycle_time) - 2.0 / tc->maxaccel * discr;
 newvel = maxnewvel = -0.5 * tc->maxaccel * tc->cycle_time +tc->maxaccel * pmSqrt(discr);

3.3. The EMC 2.1.0 TP

Compared to simple_tp, the current EMC (pre-2.1) implements a number of important new features:

  1. exact path mode
  2. synchronized motion (lathe threading, rigid tapping)
  3. blend mode (with tolerance)

These sections will describe how they work.

Exact path mode (G61)

if the end of the previous segment points in the same direction as the start of the next one there is no need to stop in the middle

Blend mode (G64 PX.XXX)

Blend corners while respecting given tolerance

LinuxCNCKnowledgeBase | RecentChanges | PageIndex | Preferences | LinuxCNC.org
This page is read-only. Follow the BasicSteps to edit pages. | View other revisions
Last edited May 20, 2011 3:01 pm by Awallin (diff)
Published under a Creative Commons License