Bahnplaner in C

Trajectory generator in C

The simple trajectory generator described here was implemented for positioning purposes in educational context, and consists of only one platform independent C header/implementation file pair. It does not contain volatile structures nor system specific synchronisation. Instead, it works with one struct type intermediate variable (type trajgen_t) that can be allocated externally and is unchanged during the update process. This variable stores configuration, internal states, input data and position output data. Added/adapted features, such as position/velocity synchronisation shall make it suitable to be integrated in LinuxCNC (EMC) context. The brief feature list:

Axes: Main axes X, Y, Z, (translation) A, B, C (rotation), Auxiliary axes U, V, W (normally collinear to X/Y/Z).
Coordinated motion planer: Line motion, arc/circle/helix motion, (spline/nurbs in test), blending, synchronised I/O channels (required for EMC compatibility), velocity synchronisation (required for EMC), position synchronisation (required for EMC).
Manual operation: Planer for command velocities (e.g. from joystick), which includes kinematics.
Joint jogging: Planer for command positions for each joint without kinematics.
Main trajectory generator interface: Finite state machine based coordinator for the "sub planers".
Interpolation: Cubic joint position interpolators affecting the final output (command positions for the closed loop controllers) for all planers.
"Callback kinematics": The internal kinematic model is the "identity" model. Th.m. the pose axes directly correspond to the joint indices. Other kinematics can be defined by configuring function pointers to the forward/ inverse/reset functionalities.
Extensive compiling options: For optimisation, all planers and the interpolators can be static __inline__'d or exported. Integer and floating point types can be specified, features, such as interpolation, synchronisation, debugging, error stringification and the like can be switched off entirely to optimise memory space and the performance.

The implementation files shown below include not only the generator files (trajgen.c/.h), but also a test program with a C++ wrapper (trajgen.hh), build scripts, Makefile and GNU Octave analysis (stats.m).

Example x/y motion of the coordinated motion generator:

(Die gewünschten allgemeinen Erläuterungen zu Bahnplanern sind hier).

Der hier beschriebene Bahngenerator wurde für Positionierungszwecke im Lehr/Forschungsbereich implementiert und für die Integration in LinuxCNC erweitert. Er besteht aus nur einem plattformunabhängigen C Header/Implementation-Dateipaar und enthält keine systemspezifischen Echtzeitsynchronisationsmechanismen. Stattdessen arbeitet er mit einer Strukturvariablen (Typ trajgen_t), die extern alloziiert wird und als Zwischenspeicher dient, der während dem Rechenvorgang nicht geändert werden darf. Sie enthält die Konfiguration, interne Zustände, Eingabewerte und Ausgabewerte.

Die Features in aller Kürze:

Achsen: Hauptachsen X, Y, Z, (Translation) A, B, C (Rotation), Hilfsachsen U, V, W (normalerweise kollinear zu den kartesischen Hauptachsen).
Koordinierte Bewegungen: Linie, Kreis/Bogen/Helix, (Spline/Nurbs im Test), Überschleifen, Geschwindigkeits-/Positionssynchronisation, synchronisierte digitale Ausgänge.
Manueller Betrieb: Aus Geschwindigkeitseingaben (z.B. von Joystick) werden mit Kinematik neue Sollpositionen erzeugt.
Motor-Jogging: Aus Geschwindigkeitseingaben werden (ohne Kinematik) unabhängig neue Sollpositionen für die einzelnen Motorachen erzeugt.
Interpolation: Kubische Spline-Interpolation kann wahlweise zwischen die Ausgabepositionen der Planer und die Motor-Sollpositionen geschaltet werden.
"Callback"-Kinematik: Das interne Default-Modell der Gerätekinematik ist das "Identity"-Modell, d.h. die Werkzeug-Achspositionen entsprechen direkt den Motorachspositionen (joints). Über konfigurierbare Funktionspointer kann das Kinematische Modell extern implementiert und eingebunden werden.
Extensive Kompilierungseinsellungen: Um dem Compiler Raum für Optimierung zu geben können alle Planer, sowie die Interpolatoren entweder exportiert oder als static __inline__ kompiliert werden. Weiterhin können Integer- und Fließkommatypen angepasst und Funktionalitäten selektiv abgeschaltet werden (Interpolation, Synchronisation, Überschleifen, Fehlertexte usw.).

Neben den Hautdateien (trajgen.c/.h) ist weiter unten auch ein Testprogramm mit C++ Wrapper (trajgen.hh) GNU-Octave Auswertung (stats.m) angegeben.

Beispiel: X/Y Bahnbewegung des Planers für koordinierte Bewegungen:

x/y positions of the trajectory

trajectory velocity/acceleration

joint velocities/accelerations

Usage

(Ab hier geht es in die Programmierdetails, und da ist die "Amtssprache" Englisch).

In the very end I cannot deny that a close look into the source code is required to use all the features properly. The inline function/type documentaion helps you there. To get "booted" read the gist of it here.

Info

The function const char* trajgen_info(char* s, unsigned length); writes a string representations of its compilation settings into a string (s). This can be handy to validate parameters of the compiled program. That looks like e.g.:

"trajgen.c/h, version 1.0b, without-0pointer-checks, no-debug, with-interpolation, with-blending, with-syncing, with-abc, with-uvw, float-type=double, max-joints=9, queue-size=32, machine-resolution=1e-9, dimensional-resolution=1e-9, angle-resolution=1e-6, blending-max-angle=85deg, straight-blending-max-angle=1deg, ..."

Initialisation and configuration

To use the trajectory generator, copy the files trajgen.h and trajgen.c in your source tree and compile latter. Include trajgen.h and allocate a variable of the type trajgen_t (e.g. tg). Then configure the planers, either directly using tg.config.(settings) or by allocating an auxiliary configuration variable of type trajgen_config_t:

#include <trajgen.h>
 
trajgen_t tg;           /* The main trajectory generator data */
 
int init_trajgen()
{
  unsigned i;
 
  /* This is where we do our configuration. You can either use a separate
   * variable like this one or use tg.config directly. It makes no difference.*/
  trajgen_config_t cfg;
 
  /* That makes you safe! The trajgen checks null pointers.
   * Any configuration trouble will cause appropriate errors. */
  memset(&cfg, 0, sizeof(trajgen_config_t));
 
  /* 1st: How many axes do we use? Less axes -> better performance.
   * Here, let's say only x,y,z */
  cfg.number_of_used_joints = 3;
 
  /* Next: The sample rate. That is the sample rate which the axes closed loop
   * controllers want new command positions. Here we choose 4kHz ... */
  cfg.sample_interval = 1.0/4e3;  /* in seconds */
 
  /* ... but the planers only run with 1kHz, the interpolators do the positions
   * in between for each joint. */
  cfg.interpolation_rate = 4;
 
  /* Here you can specify your own kinematics, but we use NULL pointers here so
   * that the internal identity kinematics are used. Actually you don't need to
   * set these values at all if you used `memset(0)` before.
   */
  cfg.kinematics_functions.forward = 0; /* function(*joints, *pose) */
  cfg.kinematics_functions.inverse = 0; /* function(*pose, *joints) */
  cfg.kinematics_functions.reset = 0;   /* function(*joints, *pose) */
 
  /* Now about the restrictions for coordinated motions. We use 100mm/s and
   * 1000mm/ss here: */
  cfg.max_coord_velocity = 100e-3;
  cfg.max_coord_acceleration = 1000e-3;
 
  /* The settings for each independent joint planer. As those are often used for
   * initialisation purposes, we restrict them all to 10mm/s and 100mm/ss: */
  for(i=0; i<cfg.number_of_used_joints; i++) {
    cfg.joints[i].max_acceleration = 100e-3;
    cfg.joints[i].max_velocity = 10e-3;
  }
 
  /* And for the manual planer we have individual settings: 100mm/ss and
   * 50mm/s: */
  cfg.max_manual_accelerations.x = 100e-3;
  cfg.max_manual_accelerations.y = 100e-3;
  cfg.max_manual_accelerations.z = 100e-3;
  cfg.max_manual_velocities.x = 50e-3;
  cfg.max_manual_velocities.y = 50e-3;
  cfg.max_manual_velocities.z = 50e-3;
 
  /* Finally we call init */
  if(trajgen_initialize(&tg, cfg)) {
    char error_text[64];
    trajgen_errstr(tg.last_error, error_text, sizeof(error_text));
    rt_printf("Initialisation error: %s\n", error_text);
    return 1;
  } else {
    rt_printf("Initialisation successful\n");
    return 0;
  }
}

Cyclic call

Supposingly you use a realtime threading system with fifos or shared memory synchronisation. No matter how this framework looks like, you have to ensure that the function trajgen_tick() is called with the sample interval that you configured. The process for each cycle is:

Check for hardware errors, such as amplifier fault, limit switches ...
Dispatch data from the realtime fifo.
Transfer input data into the trajgen_t structure tg, e.g. new command velocities from the joystick, or updated positions/velocities from synchronisation references.
CALL trajgen_tick()
Check its error
Transfer output data, such as command positions for the axis controllers and digital outputs.
Handle dispatched commands, e.g. to add line/arc/spline motion to the trajgen queue, or switch the state of the trajgen.

/* This function has to be called with the configured 4kHz. */
void cycle()
{
  /* Here some generalised dummy code to see roughly where it points to ... */
  dispatch_fifo(&cmd);
  if(check_hardware() != OK) {
    trajgen_abort();
    rt_error_message(...);
  } else {
    switch(cmd.what) {
      case ABORT:
        trajgen_abort();
        break;
      case PAUSE:
        if(trajgen_pause() != TRAJ_ERROR_OK) {
          rt_error_message(...);
        }
        break;
      default:
        /* Something to get back to later */
    }
  }
 
  /* Call trajgen_tick() */
  if(trajgen_tick() != TRAJ_ERROR_OK) {
    /* Send the error via the used realtime messaging */
    rt_error_message(tg.last_error);
    /* Abort the generator (switch to disabled state) */
    trajgen_abort();
    /* Cutoff intermediate circuit */
    set_estop(1);
    /* We even don't update the axis command positions because it could be
     * a numerical error as well */
    return;
  }
 
  /* Transfer output data ... */
  for(i=0; i<tg.config.number_of_used_joints; i++) {
    axis_controller_set_command_position(tg.joints[i].position);
  }
  if(tg.state == TRAJ_STATE_COORDINATED) {
    /* Transfer sync I/O */
    digital_output_register_set(tg.coord_planer.sync_dio_current);
 
    /* Add commands */
    switch(cmd.what) {
      case MOVE_LINE:
        if(!trajgen_queue_full()) {
          if(trajgen_add_line(cmd.to_pose, cmd.speed, cmd.accel)) {
            trajgen_abort();
            rt_error_message(...);
          }
        } else {
          rt_notify(QUEUE_FULL);
        }
        break;
      case MOVE_ARC:
        ...
      case MOVE_SPLINE:
        ...
      case SWITCH:
        ...
    }
  } else {
    switch(cmd.what) {
      ...
    }
  }
}

States

If you use more than one of the integrated generators, you need handle state dependent date in the thread function. Handling and switching states is quite easy: The important functions and variable are:

trajgen_error_t trajgen_switch_state(trajgen_state_t state)
The states you can select are TRAJ_STATE_DISABLED, TRAJ_STATE_JOINT, TRAJ_STATE_COORDINATED and TRAJ_STATE_MAN.
The state you selected is stored in trajgen_t.requested_state, and the currently active state is stored in trajgen_t.state. Latter can be as well switching state, named TRAJ_STATE_OK_TO_SWITCH, TRAJ_STATE_DISABLING, TRAJ_STATE_*_ENTER, TRAJ_STATE_*_LEAVE.
The interesting indicator you should refer to is the boolean flag trajgen_t.is_done, Before switching and after initiating a state switch you should wait until the generator has finished. Generally, when a planer is switched to, it will be implicitly reset in the ENTER state, and in the LEAVE state it will be implicitly aborted. When you switch from planer to another, the state changes from the <CURRENT_PLANER> state over <CURRENT_PLANER>_LEAVE, OK_TO_SWITCH, <NEWPLANER>_ENTER to <NEWPLANER>, where the machine velocity is zero when OK_TO_SWITCH is active. Only when the target state is active the is_done flag will be set. Calling trajgen_abort() will implicitly cause all generators, no matter if active or not, to disable or finish up, and it always switches to the state TRAJ_STATE_DISABLED.

The example shows the use of that without threading:

extern trajgen_t tg;
extern trajgen_config_t cfg;
extern void handle_error(trajgen_t* tg);
 
void sequential_example()
{
  unsigned i;
  trajgen_initialize(&_tg, cfg);
 
  if(trajgen_switch_state(TRAJ_STATE_COORDINATED)) handle_error(&tg);
 
  while(!tg.is_done) {
    if(trajgen_tick()) handle_error(&tg);
  }
 
  /* State is now  TRAJ_STATE_COORDINATED */
  for(i=0; i<10; i++) {
    /* Simply nothing will happen, even if it is selected the coord planer
     * is idle, and is_done === 1. */
    if(trajgen_tick()) handle_error(&tg);
  }
 
  if(trajgen_switch_state(TRAJ_STATE_MAN)) handle_error(&tg);
 
  while(!tg.is_done) {
    if(trajgen_tick()) handle_error(&tg);
  }
 
  /* State is now  TRAJ_STATE_MAN */
 
  ...
}

About errors

Simply said: All errors that the trajectory generator returns are critical and a reason to cut the intermediate circuit. The error type trajgen_error_t is generally used as function return, where all nonzero values indicate errors. The size is normed to uint16_t for compatibility. Additionally, the type traigen_error_details_t is a union-struct bit set with the same size, but it splits the error into a code part (12 bits) and a joint part (4 bits). Whenever an error is raised from interpolators or joint planers, the joint will be specified (value 0 to max joints). If the subsystem is not related to a joint, the value will be 0. You can get a string representation of the error with the function trajgen_errstr(). Take a look at the following example to get the gist of it:

#include "trajgen.h"
#include <stdio.h>
#include <string.h>
 
trajgen_t tg;
trajgen_config_t cfg;
trajgen_error_t erno;
char errmsg[64];
 
void try_initialize()
{
  if(!!(erno=trajgen_initialize(&tg, cfg))) {
    trajgen_errstr(tg.last_error.errno, errmsg, sizeof(errmsg));
    printf("Error: %s (%u)\n", errmsg, erno);
  } else {
    printf("Alright, init ok\n");
  }
}
 
int main()
{
  memset(&cfg, 0, sizeof(trajgen_config_t));
 
  /* Prints: "Error: [main trajgen] Null pointer" */
  if((erno=trajgen_initialize(NULL, cfg))) {
    trajgen_errstr(erno, errmsg, sizeof(errmsg));
    printf("Error: %s\n", errmsg);
  }
 
  try_initialize(); /* "Error: Invalid last used joint value" */
  cfg.number_of_used_joints = 1;
 
  try_initialize(); /* Error: [main trajgen] Invalid interpolation rate setting */
  cfg.interpolation_rate = 1;
 
  try_initialize(); /* "Error: [coord tg] Invalid acceleration" */
  cfg.max_coord_acceleration = 1000;
 
  try_initialize(); /* Error: [coord tg] Invalid velocity */
  cfg.max_coord_velocity = 100;
 
  try_initialize(); /* Error: [coord tg] Invalid sample interval */
  cfg.sample_interval = 1e-3;
 
  cfg.joints[0].max_acceleration = -5;
  try_initialize(); /* "Error: [joint tg] Invalid maximum acceleration[0]" */
 
  /* etc etc etc */
  return 0;
}

About reset

The function trajgen_reset() resets all planers to a state where they are disabled and flushed. Internal positions are set to current device positions if required. Note that reset really means reset. If you call trajgen_reset() or the reset function of any planer while the machine is moving you will cause an immediate freezing of the positions, which will lead to very high accelerations. Hence, resetting is only recommended in standstill state. When switching between the planers they will be implicitly reset in a safe situation.

Joint planers

The joint planers work with a very simple principle. They calcaulate a required velocity from the triangle ramp of the remaining distance to move and the configured axis acceleration, and truncate this velocity to the maximum configured or commanded velocity. Then they increase the current positions with s = v * t. You can change command velocity and command position any time. Complete usage:

Each joint has its own joint planer. The configuration is located in tg.config.tg[i], the data are located in the joints (tg.joints[i].tg);
Switch to state TRAJ_STATE_JOINT and wait for tg.is_done. The main coordinator will enable and reset all joint planers that have a configured max_acceleration > 0 and max_velocity > 0. During the reset, the command position will be set to the current axis position to prevent that the axes starts moving to an unexpected position. The last command_velocity is preserved.
Set the command_velocity of the planer, e.g. tg.joints[0].tg.command_velocity=10e-3.
Set the command_position of the planer, e.g. tg.joints[0].tg.command_position=10e-3.
The planer will start moving immediately. Wait until it is done, either by checking individual planers (tg.joints[0].tg.is_done) or all (tg.is_done).
The is_done flag of a joint planer is set when its current velocity is zero and the current position is the commanded position.
If you disable the planer during the motion, it will decelerate to 0 before its is_done flag is set. Note that the command position will always be set to the current position during the abort deceleration phase. Hence, aborting a joint planer is equivalent to disabling it.
The main generator functions trajgen_abort(), trajgen_pause(), trajgen_resume(), trajgen_reset() apply to all joint planers, too.
You can also use the exported joint planer functions (see trajgen.h) it you compile with the switch -DEXPORT_JOINT_TG. However, as the planer is that simple there is no need.

Simple example:

#include <trajgen.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
 
trajgen_t tg;
trajgen_config_t cfg;
double t = 0;
 
static void tick()
{
  t += tg.config.sample_interval;
  if(trajgen_tick()) exit(3);
  printf("%8.3f, %12.6f, %12.6f, %12.6f\n",
    t, tg.joints[0].position, tg.joints[0].velocity, tg.joints[0].acceleration
  );
}
 
int main()
{
  memset(&cfg, 0, sizeof(trajgen_config_t));
  cfg.number_of_used_joints = 3;
  cfg.sample_interval = 1e-3;
  cfg.interpolation_rate = 1;
  cfg.max_coord_acceleration = 1000;
  cfg.max_coord_velocity = 100;
  cfg.joints[0].max_acceleration = 100;
  cfg.joints[0].max_velocity = 10;
  if(trajgen_initialize(&tg, cfg)) return 1;
 
  /* Output list header */
  printf("     t  ,        j0s  ,        j0v  ,        j0a\n");
 
  if(trajgen_switch_state(TRAJ_STATE_JOINT)) return 2;
  while(!tg.is_done) tick();
 
  /* Check command_velocity trimmed tp 10 */
  tg.joints[0].tg.command_velocity = 20;
  tg.joints[0].tg.command_position = 2.5;
  do { tick(); } while((!tg.is_done));
 
  /* Check command_velocity === 5 */
  tg.joints[0].tg.command_velocity = 5;
  tg.joints[0].tg.command_position = 5;
  do { tick(); } while((!tg.is_done));
 
  /* Abort behaviour */
  tg.joints[0].tg.command_velocity = 5;
  tg.joints[0].tg.command_position = 10;
  do {
    if(tg.joints[0].tg.is_enabled && tg.joints[0].position >= 7.5) {
      tg.joints[0].tg.is_enabled = 0;
    }
    tick();
  } while((!tg.is_done && t < 10));
 
  /* Jogging: Set command position to Inf/-Inf */
  tg.joints[0].tg.is_enabled = 1;
  tg.joints[0].tg.command_velocity = 10;
  tg.joints[0].tg.command_position = -INFINITY;
  do {
    if(tg.joints[0].tg.is_enabled && tg.joints[0].position <= 0) {
      tg.joints[0].tg.is_enabled = 0;
    }
    tick();
  } while((!tg.is_done && t < 10));
  return 0;
}

Manual planer

The manual planer is based on command velocities of the axes X, Y, Z, A, B, C, U, V and W. As manual operation is quite different from automatic coordinated motion, this planer has individual acceleration and speed settings for each axis. These settings should be set to appropriate values for human interaction - th.m. much slower. The input velocities can come from handwheels joysticks and the like. What it does is integrating the velocities for all axes according to their accelerations while scaling down all the current velocity increment so that all axes meet their acceleration restrictions, and finally integrating the positions from the truncated velocities. The resulting pose is pass through the kinematics to yield the joint positions.

All you need to do is to switch to the state TRAJ_STATE_MAN.
You disabled it by switching back to another state. The planer decelerates to speed zero before setting the is_done status.
Whenever the velocities of all axes is zero, tg.is_done equals 1.
You set new command velocities using the pose_t variable tg.man_planer.velocity, e.g. tg.man_planer.velocity.x = 10e-3;.
The current velocity can be fetched from the variable tg.man_planer.current_velocity, but do not change this value.
trajgen_abort() applies to the manual planer, but ther is no pause nor resume.

Coordinated planer

The planer for queued automated coordinated motion (with kinematics). It basically determins the linear lengths of the segments when they are added to the queue, and generates each tick cycle a linear acceleration/velocity profile for the remaining length (to move) along the total segment length to obtain the current path progress. It then calculates the current pose (position of all axes) from the latter progress and feeds the result into the kinematics to calculate the joint command positions. See the functions trajgen_sg_tick() and trajgen_coord_tick(). All remaining functions are related to queue handling, precalculations of the segments, obtaining the pose from the progress for different motion types, modification of parameters to meet the configured or external restrictions, and error checks.

Usage:

Switch to the state TRAJ_STATE_COORDINATED and wait for tg.is_done. The planer is now reset, and the queue empty.
Use the functions trajgen_add_line(), trajgen_add_arc() or trajgen_add_spline() to push new segments into the queue. If you did not pause the planer, the motion will start immediately after adding the first segment. Wait until the planer is done. You specify speed and acceleration of the segment directly in these functions, there is no global velocity or acceleration setting. The starting point of the segment is always the end pose of the last queued segment (or the current pose if the queue is empty). Invalid or problematic arguments will raise an error.
Any error, be it a fatal numeric error or just a "queue full" will cause an immediate abort of the motion, so check trajgen_queue_full() before pushing new segments.
Depending on whatever input you have for this, you can set tg.coord_planer.override to a value between 0 and 1 to scale down the velocity.
When you set tg.coord_planer.override_enabled=0 (default after reset == 1), the described override will NOT apply to all segments that are added when this setting is 0 (all subsequent segments).
The planer has the capability to set synchronised I/O channels. The current state of these digital outputs is located in tg.coord_planer.sync_dio_current, which is a 16 bit unsigned (uint16_t). You can modify this value to whatever and whenever you like (the planer does not evaluate it). When the planer switches from a segment to the next, it can set or clear bits in sync_dio_current. To do this, set the variable tg.coord_planer.sync_dio_command (which is a struct with the fields set and clear, both are uint16_t as well) before you add a segment to the queue. The command variable will be cleared after adding the segment. Additionally, the clear field is dominant, so if there are the same bits set in set and clear, they will be cleared. When resetting the planer, the command resisters and the current state will be reset to 0x0000.
To enable blending, set a maximum path deviation with tg.coord_planer.blending_tolerance, which is in the same unit as you configured the space dimensions (e.g. millimeters). All subsequent motion will be blending if the angle between end direction of a segment and start direction of the next segment is not greater than MAX_ALLOWED_BLENDING_ANGLE_DEG (default 90º). If blending is off (tolerance 0), the planer decelerates to velocity 0 for each segment to accelerate again for the next segment. Otherwise it moves with the highest possible velocity that still meets the tolerance requirement (basically the next segment is started before the current one is finished, and the poses are superpositioned).
Velocity sychronisation is done by setting values in the structure tg.coord_planer.sync. The basic working principle is that an input reference velocity can modify the current speed of the planer between 0 and the current segments command velocity (the normal velocity that the machine moves without synchronisation). This internal override is calculated (each tick) from a the current reference velocity (reference), and a scaler from the reference velocity to the trajectory velocity (scaler): v_traj = reference * scaler. If v_traj < 0 the machine will stop, and wait to start new segments as well. And v_traj is truncated to the segment speed, of cause. Usage:
- Before adding synchronised segments, set tg.coord_planer.sync.type = TP_SYNC_VELOCITY, set tg.coord_planer.sync.scaler etc. Note that you need to set .type back to TP_SYNC_NONE when segments shall not be synchronised, it is not auto resetting like the I/Os.
- Before calling trajgen_tick(). update the reference to the current value.
- Note you can change the scaler any time, but it will affect all segments. The queue does not store individual thresholds and scalings for each segment.
- Note that the machine never moves back. Negative reference velocities (or if the product with the scaler is negative) will be interpreted as 0 and only cause the machine to stop.
- Any speed thresholding, curve lookup, minimum speed etc has to be done by modifying reference before passing to the trajgen.
- Setting the reference or scaler to NAN or a non-finite value, or the scaler to 0 will be interpreted as command speed 0.
Position synchronisation is similar to velocity syncing, except that an absolute position is the reference, and the scaler the scaler from the reference position to the trajectory (linear progress) position (not the pose). The start of synchronised motion will be postponed until the reference and the scaler are valid (means isfinite and scaler!=0). Usage:
- Set tg.coord_planer.sync.type = TP_SYNC_POSITION before adding a segment. (affects all subsequent motion until TP_SYNC_NONE or TP_SYNC_VELOCITY). Set the scaler appropriately to calculate from the reference position to the trajectory position (e.g. with the spindle slope).
- Update the reference position each cycle before calling trajgen_tick().
- If the reference has to be initialised before starting the synchronised motion, set it to NAN. When the synchronised segment is activated, the planer will wait then and set an internal offset position the first time a valid reference value is set, and work relative to this offset then. Th.m. the reference position does not need to be initialised to 0, it can be any finite number.
- Once the position motion has started, the reference and the scaler must remain valid, otherwise the planer will abort and raise an error.
- In the same manner as during velocity syncing, the machine does not move backwards.

Interpolators

The (cubic) spline interpolation is done every trajgen_tick() cycle, and work exclusively with joint positions. Each joint interpolator has a buffer of four positions from the planer, and calculate a new output position (and also velocity and acceleration) from their current progresses and the sample interval. When new position is almost exceeding the last buffer value, they request a new value from the planer, which will be invoked implicitly and push new values into the interpolator buffers. Usage notes:

Setting interpolation_rate = N in the configuration will cause the interpolators to request a new position form the planer every Nth tick, in between they interpolate (and cause less CPU load).
If you do not switch the interpolation feature completely off using the compiler switch -DNO_INTERPOLATION they will be enabled.
A positive side effect of the interpolation is a smoothing of the trajectory velocities and accelerations. A negative effect of that smoothing is a little delay compared to the "raw" planer output joint positions.
Note that the interpolators reduce the total CPU load, but they do not help at all according to meeting the calculation dead lines. If your processor does not calculate the trajectory fast enough without interpolation it will not be quick enough with interpolation neither.

Compiler switches

To be usable with different architectures and platforms, as well for debugging and performance/size optimisation, there are various compiling switches, which are all listed and documented in trajgen.h. You can #define ...
- ... the used floating point type by defining real_t to double or float (default is double).
- ... the type used for booleans (bool_t, default unsigned).
- ... the type used for bytes (byte_t, default uint8_t).
- ... the type used for bytes (byte_t, default uint8_t).
- ... the maximum configurable number of joints: JOINT_MAX_JOINTS (default 9).
- ... the size of the coordinated planer queue (TG_QUEUE_SIZE), which is fixed size, so that no dynamic allocation in the realtime/kernel space is required.
- ... the resolution of the planers (TG_RESOLUTION). The value is used in various situations if a length is zero (absolute smaller than TG_RESOLUTION).
- ... a custom sincos function. Depending on the platform they are builtin functions for that, which are faster than separately calling sin and cos. Lookup table based calculation could be used as well for fixed calculation times.
- ... a custom memory-zero function (ZERO_MEMORY(ptr, size)). Similar to memset(ptr,0,size).
- ... if the functions shall do argument NULL pointer checks. (ARG_POINTER_CHECKS). It's more a debugging/development feature.
- ... if you like to define your own joint data type, which has to be compliant with the type in trajgen.h: JOINT_DECL_NONE: Use your own, JOINT_DECL_FULL: use full internal joint declaration. Otherwise a minimal default joint declaration is used.
- ... to switch off the interpolation feature: NO_INTERPOLATION (default undefined).
- ... to switch off speed/position syncing: NO_TRAJECTORY_SYNC (default undefined).
- ... to switch off blending: NO_MOTION_BLENDING (default undefined).
- ... to export the functions of the internal functions: EXPORT_INTERPOLATORS, EXPORT_JOINT_TG, EXPORT_MANUAL_TG, EXPORT_COORD_TG (default undefined).
- ... to define a debug level: TG_DEBUG_LEVEL (0 to 2, default undefined).
- ... define own values for calculation floating point resolutions: TG_D_RES for space dimensional values (default TG_RESOLUTION) and TG_A_RES for angular/polar resolutions (default TG_RESOLUTION).

ToDo's, glitches, etc

During blending, the speed is sometimes reduced a little bit event if this is not necessary.
Spline motion is currently tested and not in the published planer yet. The function depends on a blending special "straight blending" case feature that is tested.
Maximum blending angle would be better configurable and not a compiler seting.

About performance

Compiling and running the example of the output binary of test/test-coord.cc on a Raspberry PI (ARMV7 with 700MHz and floating point unit - the slowest I have that is not a micro controller/DSP) yields an average calculation time for trajgen_tick() of 16.3us. To reduce noise from the timing measurement, the statistics of 200 test program runs (with identical commands) was evaluated. All axes, blending, syncing, etc are switched on except interpolation, so that the planer has to recalculate completely every tick. The stats show that the most calculation times are below 25us, however, there are peaks that can mess up the realtime requirements. With a RPi and e.g. a Xenomai RT patch, the maximum recommended sample rate for the planer would be about 1kHz (without interpolation, with interpolation a factor 5 is unproblematic), which is still suitable for most of the applications.

Motion profile of the example

  line {x:40, y:0, z:0, v:10, a:1000, tol:0 }
  arc  {x:30, y:10, z:0, cx:40, cy:10, cz:0, nx:0, ny:0,nz:1, v:10, a:1000, tol:0 }
  arc  {x:5, y:15, z:0, cx:25, cy:15, cz:0, nx:0, ny:0,nz:1, v:10, a:1000, tol:1 }
  line {x:5, y:10, z:0, v:10, a:1000, tol:1 }
  line {x:-25, y:10, z:0, v:100, a:1000, tol:1 }
  line {x:-50, y:0, z:0, v:100, a:250, tol:1 }
  line {x:-60, y:50, z:0, v:100, a:250, tol:1 }
  line {x:-70, y:60, z:0, v:100, a:250, tol:1 }
  line {x:-70, y:70, z:0, v:100, a:250, tol:1 }
  line {x:-60, y:80, z:0, v:100, a:250, tol:1 }
  line {x:0, y:70, z:0, v:100, a:250, tol:1 }
  line {x:60, y:60, z:0, v:100, a:250, tol:1 }
  line {x:70, y:-10, z:0, v:100, a:250, tol:1 }
  line {x:50, y:-20, z:0, v:100, a:250, tol:5 }
  line {x:-25, y:-10, z:0, v:100, a:250, tol:5 }
  line {x:0, y:-5, z:0, v:100, a:250, tol:5 }
  line {x:0, y:0, z:0, v:100, a:250, tol:5 }

Performance

  Maximum calculation time: 156.0us
  Average calculation time: 16.3us
  StdDev  calculation time: 3.0us
  Ticks < 10% of max time : 63.9%
  Ticks < 15% of max time : 99.8%
  Ticks < 20% of max time : 99.9%

The figure shows the calculation times over the motion progress.