# Trajectory generation: trapezoidal speed profile

A project log for FlexSEA: Wearable robotics toolkit

OSHW+OSS to enable the next generation of powered prosthetic limbs and exoskeletons. Let’s make humans faster/better/stronger!

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)
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 = {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
#endif
}

//Transitions:
trapez_transitions = a_t_discrete;
trapez_transitions = a_t_discrete + cte_spd_pos_discrete;
trapez_transitions = 2*a_t_discrete + cte_spd_pos_discrete;
#ifdef DEBUGGING_OUTPUT
printf("tr = %lld, tr = %lld, tr = %lld.\n", trapez_transitions, trapez_transitions, trapez_transitions);
#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)
{
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) && (pos_step <= trapez_transitions))
{
tmp_spd = last_tmp_spd;
}
}
//Negative Acceleration:
if((pos_step >= trapez_transitions) && (pos_step <= trapez_transitions))
{
last_tmp_spd = tmp_spd;
tmp_spd = last_tmp_spd - spd_inc;
}

if(pos_step <= max_steps)
{
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

#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.

## Discussions 