Implementing a Shaper

Documentation

Development

Introduction

This guide will walk you through the process of implementing a new input shaper. We will create an input shaper that adds a sine wave to the position with a user-defined frequency and magnitude.

If you have not yet set up a local copy of the simulator, then you should first follow the development setup guide.

The following list gives a brief overview of the capabilities of input shapers in Prunt and the constraints placed on them:

Input shapers are implemented as arbitrary functions which receive a series of positions at fixed time intervals for a single axis.
Input shapers must output a position each time a new position is received.
Input shapers may define arbitrary persistent internal variables as required for processing.
Prunt can shift inputs forwards or backwards an arbitrary amount in time to allow for the synchronisation of different shapers (practically limited by the stack size, which may be trivially changed in prunt-step_generator-generator.ads).
Prunt can send a series of extra positions at the end of a motion block to allow for input shapers to flush their internal buffers.

Adding the shaper parameters

The shaper parameters should be added to prunt-input_shapers.ads. We will also add these options to the GUI later.

Start by adding the Sine_Wave_Demo to the Shaper_Kind enumeration type:

type Shaper_Kind is (No_Shaper, Zero_Vibration, Extra_Insensitive, Sine_Wave_Demo);

Continue by adding our required parameters to the variant part of Shaper_Parameters:

type Shaper_Parameters (Kind : Shaper_Kind := No_Shaper) is record
   case Kind is
      when No_Shaper =>
         null;
      when Zero_Vibration =>
         ...
      when Sine_Wave_Demo =>
         Sine_Wave_Magnitude : Length;
         Sine_Wave_Frequency : Frequency;
   end case;
end record;

The types we are using above are floating-point numbers defined in prunt.ads. These types automatically have dimensional analysis applied to them and will cause compile-time errors to be emitted in the case of a dimension mismatch.

Implementing the shaper

Package specification

We will start by implementing the package specification for the shaper in prunt-input_shapers-sine_wave_demo_shapers.ads. Refer to the commentary below the following code for details on what each part does.

with Prunt.Input_Shapers.Shapers;

package Prunt.Input_Shapers.Sine_Wave_Demo_Shapers is

   type Sine_Wave_Demo_Shaper
     (Input_Offset : Cycle_Count; Extra_End_Time : Cycle_Count; Buffer_Size : Cycle_Count)
   is
     new Shapers.Shaper with private;

   function Create
     (Parameters         : Shaper_Parameters;
      Interpolation_Time : Time;
      Start_Position     : Length)
      return Sine_Wave_Demo_Shaper with
     Pre => Parameters.Kind = Sine_Wave_Demo;

   overriding function Do_Step (This : in out Sine_Wave_Demo_Shaper; Step : Length) return Length;

private

   type Buffer_Array is array (Cycle_Count range <>) of Length;

   type Sine_Wave_Demo_Shaper
     (Input_Offset : Cycle_Count; Extra_End_Time : Cycle_Count; Buffer_Size : Cycle_Count)
   is
   new Shapers.Shaper (Input_Offset => Input_Offset, Extra_End_Time => Extra_End_Time) with record
      Buffer               : Buffer_Array (0 .. Buffer_Size);
      Current_Buffer_Index : Cycle_Count;
      Next_Sine_Input      : Dimensionless;
      Sine_Wave_Magnitude  : Length;
      Sine_Wave_Frequency  : Frequency;
   end record;

end Prunt.Input_Shapers.Sine_Wave_Demo_Shapers;

Partial declaration of `Sine_Wave_Demo_Shape`

Input_Offset is passed to a discriminant of the parent type and defines how many cycles the input to Do_Step will be delayed by. The applied offset should result in the predicted movement of the axis matching as closely as possible to the movement of an ideal axis without shaping to allow axes with different shaper parameters to stay synchronised with each other. This value can be (and usually should be) a negative value to receive inputs that are a given number of cycles into the future. Note that Input_Offset is relative to offsets requested by other shapers, so requesting an offset of 1000 will not guarantee 1000 repeats of the initial position if all other shapers also request an offset of 1000.

Extra_End_Time is also passed to a discriminant of the parent type. After all moves are complete, Do_Step will be called Extra_End_Time times with a repeat of the finishing position to allow for flushing of any remaining outputs that have been buffered internally by the shaper.

Buffer_Size is a discriminant that we will use to create a buffer to demonstrate how Prunt can handle a delay introduced by our shaper by shifting moves forward in time.

Function declarations

The Create procedure used to create a new instance is created for each motion block (i.e. a large array of moves sharing the same kinematic parameters). This function is not overriding as dispatching is performed manually based on the shaper parameters. Interpolation_Time is the fixed time step between inputs passed to Do_Step and Start_Position is the position that the axis is idle at before the first call to Do_Step. The precondition here is not required but will help to prevent issues later.

We do not technically need to declare the Do_Step function here, but it makes things easier to keep track of. The overriding qualifier causes the compiler to emit an error if this function does not match the abstract function declared on the parent type.

Private declarations

The Buffer_Array type is an array of arbitrary size which may have its lower and upper index bounds set at runtime to any value of Cycle_Count.

Full declaration of `Sine_Wave_Demo_Shaper`

is new Shapers.Shaper (Input_Offset => Input_Offset, Extra_End_Time => Extra_End_Time) passes some of the discriminants to the parent type and with record allows us to append our own components to the type. Discriminants are similar to record components except for the fact that they cannot be changed after the record is initialised and may be used within the declaration of other record components, including components without a size known at compile time.

We use one of these discriminants in Buffer : Buffer_Array (0 .. Buffer_Size);, which declares an array which contains Buffer_Size + 1 values. This buffer will be a ring buffer which adds a delay between inputs and outputs. The mismatch in size here is just to make some later code a bit simpler. We can not subtract from a value that comes from a discriminant (or perform any other operation) to get the correct size while setting the lower bound to 0.

Current_Buffer_Index keeps track of our current position in the ring buffer and the other components are used to handle the sine wave output.

Package body

Now that we have a package specification, we can implement the package definition in prunt-input_shapers-sine_wave_demo_shapers.adb. Refer to the commentary below the following code for details on what each part does.

with Ada.Numerics.Generic_Elementary_Functions;
with Ada.Numerics; use Ada.Numerics;

package body Prunt.Input_Shapers.Sine_Wave_Demo_Shapers is

   package Dimensionless_Math is new Ada.Numerics.Generic_Elementary_Functions (Dimensionless);
   use Dimensionless_Math;

   function Create
     (Parameters         : Shaper_Parameters;
      Interpolation_Time : Time;
      Start_Position     : Length)
      return Sine_Wave_Demo_Shaper
   is
   begin
      if Parameters.Kind /= Sine_Wave_Demo then
         raise Constraint_Error with "Wrong create function called for given parameters.";
      end if;

      return
        (Input_Offset         => -100,
         Extra_End_Time       => 100,
         Buffer_Size          => 100,
         Buffer               => (others => Start_Position),
         Current_Buffer_Index => 0,
         Next_Sine_Input      => 0.0,
         Sine_Magnitude       => Parameters.Sine_Wave_Magnitude,
         Sine_Step            => Parameters.Sine_Wave_Frequency * Interpolation_Time * 2.0 * Pi);
   end Create;

   overriding function Do_Step (This : in out Sine_Wave_Demo_Shaper; Step : Length) return Length
   is
      Result : constant Length :=
        This.Buffer (This.Current_Buffer_Index) + Sin (This.Next_Sine_Input) * This.Sine_Magnitude;
   begin
      This.Current_Buffer_Index := (This.Current_Buffer_Index + 1) mod This.Buffer_Size;
      This.Buffer (This.Current_Buffer_Index) := Step;

      This.Next_Sine_Input :=
        Dimensionless'Remainder (This.Next_Sine_Input + This.Sine_Step, 2.0 * Pi);

      return Result;
   end Do_Step;

end Prunt.Input_Shapers.Sine_Wave_Demo_Shapers;

Imports and package instantiation

We import Ada.Numerics.Generic_Elementary_Functions to get access to a sine function and Ada.Numerics to get access to a pi constant.

Due to Ada’s strong typing we can not just have a sine function that works on all floating-point types. Instead we need to instantiate the generic package Generic_Elementary_Functions with the Dimensionless type as a parameter to get a sine function that takes a Dimensionless type as an input and returns the same as an output.

`Create` function

We start with an explicit check that the parameters are the right kind for this procedure. This is not required but will help to prevent issues later.

Next we create and return the shaper instance using a record aggregate. We request for Prunt to offset the inputs so that they arrive 100 cycles sooner than they otherwise would and add a 100 cycle delay buffer filled with the starting position for the sake of demonstration. As we are introducing a 100 cycle delay we need to also set Extra_End_Time to 100. We would have to set this higher if this were a shaper that applied smoothing or something similar.

We can see a benefit of using types with dimensional analysis here. If we made the mistake of thinking that Interpolation_Time was a frequency rather than a time and wrote Sine_Wave_Frequency / Interpolation_Time then we would get a compiler error pointing to the mistake:

prunt-input_shapers-sine_wave_demo_shapers.adb:21:09: error: dimensions mismatch in record aggregate
prunt-input_shapers-sine_wave_demo_shapers.adb:28:92: error: expected dimension [], found [Time**(-2)]

We can use Pi without a generic package here as Pi is a numeric literal defined without a type. This only works for dimensionless types, if we wanted to assign to a value of type Area we would need to write something similar to Pi * mm * mm to get a value of pi mm² or (Pi * mm)**2 to get the area of a square with pi mm sides. Dimensioned types work correctly with the exponent operator (**) as long as the exponent is an integer or a fraction with a numerator and denominator which are integers (e.g. 2 * mm**(1/3)).

`Do_Step` function

The Do_Step function is very simple. We get the next value from the buffer, add the dimensionless sine output multiplied by a length to it, and set that as the result. We then add the next value into the buffer and update the sine input before returning the result.

Integrating the shaper

Adding a call to `Create`

We need to add a small amount of code to allow Prunt to call our new Create function. This is straightforward and is done in the Create function in prunt-input_shapers-shapers.adb as shown below:

function Create
  (Parameters         : Axial_Shaper_Parameters;
   Interpolation_Time : Time;
   Initial_Position   : Position)
   return Axial_Shapers
is
   ...
begin
   for A in Axis_Name loop
      case Parameters (A).Kind is
         when No_Shaper | Zero_Vibration | Extra_Insensitive =>
            Result.Shapers.Insert
              (A, Basic_Shapers.Create (Parameters (A), Interpolation_Time, Initial_Position (A)));
         when Sine_Wave_Demo =>
            Result.Shapers.Insert
              (A,
               Sine_Wave_Demo_Shapers.Create
                 (Parameters (A), Interpolation_Time, Initial_Position (A)));
      end case;
   end loop;

We also need to add with Prunt.Input_Shapers.Sine_Wave_Demo_Shapers; to the top of the file.

Adding parameters to the GUI

This step is more involved than the previous step. We start by finding Shaper_Sequence in prunt-config.adb and then adding a new shaper type alongside the existing ones. After doing this, Shaper_Sequence should look like this:

Shaper_Sequence : constant Property_Parameters_Access :=
  Variant
    ("Input shaping method used for this axis.", --  Description
     "No shaper", --  Default value
     ["No shaper" =>
        Sequence
          ("No input shaping will be applied to this axis.",
           []),
      "Zero vibration (ZV/ZVD/ZVDD/etc.)" =>
        Sequence (...),
      "Extra insensitive (EI/2HEI/3HEI)" =>
        Sequence (...),
      "Sine wave demo" =>
        Sequence
          ("Adds a sine wave to the position.",
           ["Frequency" =>
              Float
                ("Frequency of the sine wave.",
                 Default => 200.0,
                 Min     => 0.0,
                 Max     => 1.0E100,
                 Unit    => "Hz"),
            "Magnitude" =>
              Float
                ("Magnitude of the sine wave,",
                 Default => 0.1,
                 Min     => 0.0,
                 Max     => 100.0,
                 Unit    => "mm")])]);

Next we move on to the JSON_To_Config function in the Config_File protected type in the same file. Here we need to find the code that parses the shaper parameters (search for Get (Data, "Input shaping$" & A'Image)) and add a new elsif for our new shaper, resulting in the following:

if Get (Data, "Input shaping$" & A'Image) = "No shaper" then
   Config.Shapers (A) := (Kind => No_Shaper);
elsif Get (Data, "Input shaping$" & A'Image) = "Zero vibration (ZV/ZVD/ZVDD/etc.)" then
   ...
elsif Get (Data, "Input shaping$" & A'Image) = "Extra insensitive (EI/2HEI/3HEI)" then
   ...
elsif Get (Data, "Input shaping$" & A'Image) = "Sine wave demo" then
   Config.Shapers (A) :=
     (Kind                => Sine_Wave_Demo,
      Sine_Wave_Frequency =>
        Get (Data, "Input shaping$" & A'Image & "$Sine wave demo$Frequency") * hertz,
      Sine_Wave_Magnitude =>
        Get (Data, "Input shaping$" & A'Image & "$Sine wave demo$Magnitude") * mm);
else
   raise Constraint_Error;
end if;

Testing

That covers everything required to implement a new shaper. You can now run the simulator as described here and test your new shaper after applying it to an axis on the configuration page.

Development Setup

Implementing a Shaper

Introduction

Adding the shaper parameters

Implementing the shaper

Package specification

Partial declaration of Sine_Wave_Demo_Shape

Function declarations

Private declarations

Full declaration of Sine_Wave_Demo_Shaper

Package body

Imports and package instantiation

Create function

Do_Step function

Integrating the shaper

Adding a call to Create

Adding parameters to the GUI

Testing

Partial declaration of `Sine_Wave_Demo_Shape`

Full declaration of `Sine_Wave_Demo_Shaper`

`Create` function

`Do_Step` function

Adding a call to `Create`