Picture yourself in your car, going from one stop sign to the next. The fastest way to cover that distance would be to put the pedal to the ground, go as fast as possible and, at the last second, brake as hard as possible. Why isn’t that the prevailing way of driving? First, the accelerations would be way too high, you’d go from pulled in your seat to banging your head on the steering wheel. Second, unless you are driving a race car, the dynamics at play will prevent you from instantaneously reaching your top speed. You’ll need time to accelerate and decelerate.

A typical way to control robotic joints is to use a trapezoidal speed profile, which is quite similar to the profiles you’d get by driving your car: acceleration, constant speed, and deceleration.

In terms of math and logic the code is extremely simple. I wrote two Matlab scripts; the first one has all the math, and the second one loads experiments and calls the first one. To understand how I implemented this, read this code first, then dig in the C files.

Main function:

function [ spd, pos, acc ] = trapez_motion_2( pos_i, pos_f, spd_i, spd_max, a )
%Based on trapez_motion_1.m Now in a function
% Limitation(s): assumes 0 initial speed (spd_i is useless, should be 0)

    dt = 0.01;  %10ms
    skip_sspeed = 0;

    d_pos = pos_f - pos_i;          %Difference in position
    d_spd = spd_max - spd_i;        %Difference in speed
    a_t = d_spd / a;                %How long do we accelerate?
    a_t_discrete = a_t / dt        %   (in ticks)
    spd_inc = d_spd / a_t_discrete %Every tick, increase spd by
    %Position from acc:
    acc_pos = 0;
    for i = 1:a_t_discrete
        acc_pos = acc_pos + (i*spd_inc*dt);
    end
    acc_pos
    
    %It's possible to overshoot position if the acceleration is too low.
    %In that case we should sacrifice the top speed
    if((2*acc_pos) > d_pos)
        disp('Position overshoot')
        
        spd_max = sqrt(a*d_pos)
        
        %Redo the initial math:
        d_spd = spd_max - spd_i;        %Difference in speed
        a_t = d_spd / a;                %How long do we accelerate?
        a_t_discrete = a_t / dt        %   (in ticks)
        spd_inc = d_spd / a_t_discrete %Every tick, increase spd by
        %Position from acc:
        acc_pos = 0;
        for i = 1:a_t_discrete
            acc_pos = acc_pos + (i*spd_inc*dt);
        end
        acc_pos
    end
    
    cte_spd_pos = d_pos - 2*acc_pos;
    cte_spd_pos_discrete = (cte_spd_pos/spd_max) / dt
    if(cte_spd_pos_discrete < 0)
        disp('No steady speed!')
        skip_sspeed = 1;
    end

    %At this point all the parameters are computed, we can get the 3 plots
    vector_length = 2*length(a_t_discrete) + length(cte_spd_pos_discrete);
    spd = zeros(1,vector_length);
    spd(1) = spd_i;
    pos = zeros(1,vector_length);
    pos(1) = pos_i;
    acc = zeros(1,vector_length);
    acc(1) = a;
    %Acceleration:
    for i = 1:a_t_discrete
        tmp_spd = spd(end) + spd_inc;
        tmp_pos = sum(spd)*dt;
        tmp_acc = a;

        spd = [spd tmp_spd];
        pos = [pos tmp_pos];
        acc = [acc tmp_acc];
    end
    if(skip_sspeed == 0)
        %Constant speed
        for i = 1:cte_spd_pos_discrete
            tmp_spd = spd(end);
            tmp_pos = sum(spd)*dt;
            tmp_acc = 0;

            spd = [spd tmp_spd];
            pos = [pos tmp_pos];
            acc = [acc tmp_acc];
        end
    end
    %Negative Acceleration:
    for i = 1:a_t_discrete
        tmp_spd = spd(end) - spd_inc;
        tmp_pos = sum(spd)*dt;
        tmp_acc = -a;

        spd = [spd tmp_spd];
        pos = [pos tmp_pos];
        acc = [acc tmp_acc];
    end
end

And now the code the function that calls the previous script:

close all;
clear all;
clc;

% exp(1,:) = [1000,3000,0,700,150];
% exp(2,:) = [0,100,0,10,1];
% exp(3,:) = [0,500,0,10,1];
% exp(4,:) = [0,500,0,10,1];
exp(5,:) = [0,500,10,20,1];
% exp(6,:) = [0,300,0,15,5];
% exp(7,:) = [0,100,0,5,10];
% exp(8,:) = [0,5,0,5,1];
dim = size(exp);
max_i = dim(1);

for i = 1:max_i
    
    str = sprintf('Experiment #%i',i);
    disp(str)
    [spd1 pos1 acc1] = trapez_motion_2(exp(i,1),exp(i,2),exp(i,3),exp(i,4),exp(i,5));

    figure()
    subplot(3,1,1)
    plot(acc1, 'r')
    title('Acceleration')
    subplot(3,1,2)
    plot(spd1, 'r')
    title('Speed')
    subplot(3,1,3)
    plot(pos1, 'r')
    title('Position')
    
end

The beauty of Matlab is that it’s so powerful and simple. You can quickly prove your algorithm… but will it work in real life? In that case, my first C implementation had issues with integer rounding that created a lot of problems.

Those sharp transitions created a lot of vibration and instability on the prosthetic knee I was using as a test bench!

Here's the full C implementation, starting with trapez.h and followed by trapez.c:

//****************************************************************************
// MIT Media Lab - Biomechatronics
// Jean-Francois (Jeff) Duval
// jfduval@mit.edu
// 05/2015
//****************************************************************************
// trapez: trapezoidal trajectory generation
//****************************************************************************

#ifndef TRAPEZ_H_
#define TRAPEZ_H_

//****************************************************************************
// Include(s):
//****************************************************************************

//****************************************************************************
// Public Function Prototype(s):
//****************************************************************************

long long trapez_gen_motion_1(long long pos_i, long long pos_f, long long spd_max, long long a);
long long trapez_get_pos(long long max_steps);

//****************************************************************************
// Definition(s):
//****************************************************************************

#define TRAPEZ_DT           	0.001		//Trapezoidal timebase. Has to match hardware!
#define TRAPEZ_ONE_OVER_DT  	1000
#define SPD_FACTOR          	10000		//Scaling for integer
#define ACC_FACTOR          	10000

#endif // TRAPEZ_H_

//****************************************************************************
// MIT Media Lab - Biomechatronics
// Jean-Francois (Jeff) Duval
// jfduval@mit.edu
// 05/2015
//****************************************************************************
// trapez: trapezoidal trajectory generation
//****************************************************************************

//Work based on trapez_gen_x.m, translated in C
//JFDuval 06/17/2014

//****************************************************************************
// Include(s)
//****************************************************************************

//Comment the next line to use in your application:
//#define DEBUGGING_OUTPUT

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include "trapez.h"
//#include "main.h"


//****************************************************************************
// Local variable(s)
//****************************************************************************

//Common variables - careful, do not change "manually"!
long long d_pos = 0, d_spd = 0, a_t = 0, a_t_discrete = 0, spd_inc = 0, acc_pos = 0, acc = 0;
long long init_pos = 0, cte_spd_pos = 0, cte_spd_pos_discrete = 0;
long long skip_sspeed = 0;
long long pos_step = 0;
long long trapez_transitions[3] = {0,0,0};
long long sign = 0;

//****************************************************************************
// Private Function Prototype(s):
//****************************************************************************

static long long trapez_compute_params(long long pos_i, long long pos_f, long long spd_max, long long a);

//****************************************************************************
// Public Function(s)
//****************************************************************************

//Based on trapez_motion_2.m
//Assumes 0 initial speed
long long trapez_gen_motion_1(long long pos_i, long long pos_f, long long spd_max, long long a)
{
	long long abs_d_pos = 0, abs_acc_pos = 0, dual_abs_acc_pos = 0;

    //spd_max & a have to be positive
    spd_max = llabs(spd_max);
    a = llabs(a);

    //Compute parameters (in global variables)
    trapez_compute_params(pos_i, pos_f, spd_max, a);

    //Absolute values
    abs_d_pos = llabs(d_pos);
    abs_acc_pos = llabs(acc_pos);
    dual_abs_acc_pos = 2*abs_acc_pos;

    #ifdef DEBUGGING_OUTPUT
    printf("d_pos = %lld, abs_d_pos = %lld.\n", d_pos, abs_d_pos);
    printf("1) acc_pos = %lld, abs_acc_pos = %lld.\n", acc_pos, abs_acc_pos);
    #endif

    //It's possible to overshoot position if the acceleration is too low.
    //In that case we should sacrifice the top speed
    if(dual_abs_acc_pos > abs_d_pos)
    {
        #ifdef DEBUGGING_OUTPUT
        printf("Position overshoot.\n");
        #endif

        //New top speed:
        spd_max = sqrt(a*abs_d_pos);
        #ifdef DEBUGGING_OUTPUT
        printf("New spd_max: %lld.\n", spd_max);
        #endif

        //Redo the initial math:
        //Compute parameters (in global variables)
        trapez_compute_params(pos_i, pos_f, spd_max, a);

        //Absolute values (they probably changed)
        abs_d_pos = abs(d_pos);
        abs_acc_pos = abs(acc_pos);
        dual_abs_acc_pos = 2*abs_acc_pos;
    }

    //Plateau - constant speed
    #ifdef DEBUGGING_OUTPUT
    printf("d_pos = %lld, abs_d_pos = %lld.\n", d_pos, abs_d_pos);
    printf("2) acc_pos = %lld, abs_acc_pos = %lld.\n", acc_pos, abs_acc_pos);
    #endif

    cte_spd_pos = abs_d_pos - (2*abs_acc_pos);
    cte_spd_pos_discrete = (SPD_FACTOR*cte_spd_pos/spd_max)*TRAPEZ_ONE_OVER_DT;
    cte_spd_pos_discrete = cte_spd_pos_discrete / SPD_FACTOR;
    #ifdef DEBUGGING_OUTPUT
    printf("cte_spd_pos = %lld, cte_spd_pos_discrete = %lld.\n", cte_spd_pos, cte_spd_pos_discrete);
    #endif
    if(cte_spd_pos_discrete < 0)
    {
        cte_spd_pos_discrete = 0;
        #ifdef DEBUGGING_OUTPUT
        printf("No steady speed!\n");
        #endif
    }

    //Transitions:
    trapez_transitions[0] = a_t_discrete;
    trapez_transitions[1] = a_t_discrete + cte_spd_pos_discrete;
    trapez_transitions[2] = 2*a_t_discrete + cte_spd_pos_discrete;
    #ifdef DEBUGGING_OUTPUT
    printf("tr[0] = %lld, tr[1] = %lld, tr[2] = %lld.\n", trapez_transitions[0], trapez_transitions[1], trapez_transitions[2]);
    #endif
    pos_step = 0;   //Variable used to output the current position command

    return (2*a_t_discrete + cte_spd_pos_discrete); //Returns the number of steps
}

//Runtime function - gives the next position setpoint
long long trapez_get_pos(long long max_steps)
{
    static long long tmp_spd = 0, last_tmp_spd = 0, tmp_pos = 0;
    long long position = 0;
    static long long pos_integral = 0;

    //At this point all the parameters are computed, we can get the 3 plots

    //First time:
    if(pos_step == 0)
    {
        tmp_spd = 0;
        last_tmp_spd = 0;
        tmp_pos = 0;
        pos_integral = 0;
        #ifdef DEBUGGING_OUTPUT
        printf("pos_step = 0, pos_integral = %lld.\n", pos_integral);
        #endif
    }

    //Acceleration:
    if(pos_step <= trapez_transitions[0])
    {
        last_tmp_spd = tmp_spd;
        tmp_spd = last_tmp_spd + spd_inc;
    }
    //if(skip_sspeed == 0)    //ToDo useful?
    {
        //Constant speed
        //last_tmp_spd = tmp_spd;
        if((pos_step >= trapez_transitions[0]) && (pos_step <= trapez_transitions[1]))
        {
            tmp_spd = last_tmp_spd;
        }
    }
    //Negative Acceleration:
    if((pos_step >= trapez_transitions[1]) && (pos_step <= trapez_transitions[2]))
    {
        last_tmp_spd = tmp_spd;
        tmp_spd = last_tmp_spd - spd_inc;
    }

    if(pos_step <= max_steps)
    {
        //Ready for next one.
        pos_step++;

        //Common math:
        pos_integral += tmp_spd;
        tmp_pos = pos_integral/(TRAPEZ_ONE_OVER_DT * SPD_FACTOR);
        position = tmp_pos + init_pos;
        #ifdef DEBUGGING_OUTPUT
        if(pos_step < 10)
        	printf("pos_step = %lld, pos_integral = %lld, position = %lld.\n", pos_step, pos_integral, position);
        #endif
    }
    else
    {
        position = tmp_pos + init_pos;
    }

    //New setpoint
    return position;
}

//****************************************************************************
// Private Function(s)
//****************************************************************************

//Computes all the parameters for a new trapezoidal motion trajectory
//Called by trapez_gen_motion_1()
static long long trapez_compute_params(long long pos_i, long long pos_f, long long spd_max, long long a)
{
    long long tmp = 0, i = 0;

    //Sign
    if(pos_f < pos_i)
        sign = -1;
    else
        sign = 1;

    acc = a;
    init_pos = pos_i;
    d_pos = pos_f - pos_i;                                  //Difference in position
    d_spd = spd_max ;                                       //Difference in speed
    a_t = (ACC_FACTOR*d_spd) / a;                                        //How long do we accelerate?
    a_t_discrete = a_t * TRAPEZ_ONE_OVER_DT / ACC_FACTOR;                //   (in ticks)
    //a_t_discrete = a_t; //Simplification of *100/100
    spd_inc = (sign*SPD_FACTOR*d_spd) / a_t_discrete;       //Every tick, increase spd by
    #ifdef DEBUGGING_OUTPUT
    printf("d_spd = %lld, a_t_discrete = %lld, spd_inc = %lld, d_pos = %lld.\n", d_spd, a_t_discrete, spd_inc, d_pos);
    #endif

    acc_pos = 0;
    for(i = 0; i < a_t_discrete; i++)
    {
        tmp += spd_inc; //tmp = i*spd_inc;
        acc_pos = acc_pos + tmp;
    }
    acc_pos = acc_pos / (SPD_FACTOR * TRAPEZ_ONE_OVER_DT); //Combine terms
    #ifdef DEBUGGING_OUTPUT
    printf("acc_pos = %lld (2x = %lld), %f%% of d_pos.\n", acc_pos, (2*acc_pos), (float)(2*acc_pos*100/d_pos));
    #endif

    return 0;
}

As you can see, I’m multiplying everything by a big factor to use integer math (way more efficient than floating point math) and I scale it back down once I’m done computing.

At runtime, in my time shared while() loop (see Managing timing: how to sequence tasks for more details) I can get a new setpoint and use it for my controllers. The trajectory is only calculated when I initiate a new motion (usually after receiving a command from FlexSEA-Manage or -Plan).

				//Case 5: Quadrature encoder & Position setpoint
				case 5:
					
					#ifdef USE_QEI1
				
					//Refresh encoder readings
					encoder_read();
						
					#endif	//USE_QEI1		
					
					#ifdef USE_TRAPEZ	
				
					if((ctrl.active_ctrl == CTRL_POSITION) || (ctrl.active_                                        ctrl == CTRL_IMPEDANCE))
					{	
						//Trapezoidal trajectories (can be used for bot                                                h Position & Impedance)				
						ctrl.position.setp = trapez_get_pos(steps);	                                        //New setpoint
					}
					
					#endif	//USE_TRAPEZ	
			
					break;
				
				//Case 6: P & Z controllers, 0 PWM	
				case 6:
					
					#ifdef USE_TRAPEZ	
					
					if(ctrl.active_ctrl == CTRL_POSITION)
					{
						motor_position_pid(ctrl.position.setp, ctrl.pos                                                ition.pos);
					}
					else if(ctrl.active_ctrl == CTRL_IMPEDANCE)
					{
						//Impedance controller
						motor_impedance_encoder(ctrl.impedance.setpoint                                                _val, ctrl.impedance.actual_val);
					}
					
					#endif	//USE_TRAPEZ
					
					//If no controller is used the PWM should be 0:
					if(ctrl.active_ctrl == CTRL_NONE)
					{
						motor_open_speed_1(0);
					}
					
					break;

And that’s it! You now have a complete example of a trajectory generator, both in Matlab and C, that works in the real life.

Now, please note that a better strategy is to use s-curve trajectories to minimize the jerk. I didn’t implement that yet, but feel free to contribute to FlexSEA by coding it! Some reference here: On Algorithms for Planning S-curve Motion Profiles.