simple_tp
-
- 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.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.
- current state of the machine/trajectory controller
- currentPos (where are we now)
- goalPos (where we are going)
- done (are we done with the current motion)
- state flags: aborting, pausing, motionType (TC_LINEAR or TC_CIRCULAR).
- Motion queue information
- queue is a pointer to the motion queue (of type TC_QUEUE_STRUCT)
- queueSize indicates the length of the queue
- nextId has the id of the next motion
- execId the id of the currently executing motion
- termCond is either TC_TERM_COND_STOP or TC_TERM_COND_BLEND
- depth indicates the total number of queued motions
- activeDepth the number of motions for blending.
- trajectory constraints:
- cycleTime (TRAJ_PERIOD from the .ini file?)
- vMax
- ini_maxvel
- vScale (feed override)
- vRestore (feed override to restore to after pause)
- aMax max linear acceleration
- vLimit max linear velocity
- wMax max rotary velocity
- wDotMax? max rotary acceleratio
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:
- very simple functions for setting the TP_STRUCT state (basic):
- extern int tpSetCycleTime?(TP_STRUCT * tp, double secs);
- extern int tpSetVmax?(TP_STRUCT * tp, double vmax, double ini_maxvel);
- extern int tpSetVlimit?(TP_STRUCT * tp, double limit);
- extern int tpSetVscale?(TP_STRUCT * tp, double scale); /* 0.0 .. large */
- extern int tpSetAmax?(TP_STRUCT * tp, double amax);
- extern int tpSetId?(TP_STRUCT * tp, int id);
- extern int tpGetExecId?(TP_STRUCT * tp);
- extern int tpSetTermCond?(TP_STRUCT * tp, int cond);
- extern int tpSetPos?(TP_STRUCT * tp, EmcPose? pos);
- Functions for setting the TP_STRUCT state (with some functionality):
- extern int tpPause(TP_STRUCT * tp);
- extern int tpResume(TP_STRUCT * tp);
- extern int tpAbort(TP_STRUCT * tp);
- Functions for reading the TP_STRUCT state:
- extern EmcPose? tpGetPos?(TP_STRUCT * tp);
- extern int tpIsDone?(TP_STRUCT * tp);
- extern int tpIsPaused?(TP_STRUCT * tp);
- extern int tpQueueDepth?(TP_STRUCT * tp);
- extern int tpActiveDepth?(TP_STRUCT * tp);
- extern int tpGetMotionType?(TP_STRUCT * tp);
- Advanced functions:
- extern int tpAddLine?(TP_STRUCT * tp, EmcPose? end, int type);
- extern int tpAddCircle?(TP_STRUCT * tp, EmcPose? end, PmCartesian? center, PmCartesian? normal, int turn, int type);
- extern int tpRunCycle?(TP_STRUCT * tp);
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.
- FIXME: but nothing from cubic.c is ever called from tp.c ??
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:
- a pointer queue (actually an array of pointers) which points to the TC_STRUCTs it contains
- size indicates the queue size
- _len indicates the number of motions in the queue
- start and end indicate the next motion to get and the next to put
- allFull indicates that the queue is full.
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
- cycle_time (TRAJ_PERIOD ?),
- progress (where are we in the segment? 0..target),
- target (segment's end position) ,
- reqvel (velocity requested by F word, calc'd by task),
- maxaccel (accel calc'd by task),
- feed_override (feed override requested by user),
- maxvel (max possible vel (feed override stops here)),
- currentvel (keep track of current step (vel * cycle_time)),
- id (segment's serial number),
- coords (start and end positions of segment),
- motion_type (TC_LINEAR or TC_CIRCULAR),
- active (is this segment being executed?),
- canon_motion_type (this motion is due to which canon function?)
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?:
- interpreter calls tpAddLine?/tpAddCircle? according to what it gets as input (MDI, G-code)
- tpRunCycle? is called every TRAJ_PERIOD. It updates currentPos
- 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
- if(!tc) (motion queue is empty i.e. end of program or queue starvation)
- initialize a new empty motion queue
- stop the machine by doing tp->goalPos = tp->currentPos and tp->done = 1
- call tpResume(tp) and then sit and wait for something to be added to the motion queue
- if(tp->aborting) ( an abort message has come)
- set the current velocity to 0.0
- stop (tp->goalPos = tp->currentPos) and tp->done = 1
- if (tc->target == tc->progress) we are done with the current segment
- remove the current segment from the motion queue
- get the next segment from the motion queue
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:
- If there is a cruise phase
accelerate up to desired feedrate
cruise phase
deccelerate down to zero feed
- 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
or
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.3. The EMC 2.1.0 TP
Compared to simple_tp, the current EMC (pre-2.1) implements a number of important new features:
- exact path mode
- synchronized motion (lathe threading, rigid tapping)
- 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