|
See also blog post: http://www.anderswallin.net/2011/05/emc2-tpruncycle-revisited/ |
 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); |
![[Home]](/chips.gif)
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)
The content of TP_STRUCT seems to fall roughly into three different categories.
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.
TC_QUEUE_STRUCT is a queue of TC_STRUCT elements. TC_QUEUE_STRUCT has:
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.
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 ?)
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:
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 )
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.
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);
Compared to simple_tp, the current EMC (pre-2.1) implements a number of important new features:
These sections will describe how they work.