SISO control (trajectory following)

This notebook illustrates examples of single-input, single-output trajectory following control, and assumes that all definitions in "manipulator_defs.nb" have been evaluated. All user input sections are highlighted as cell boxes.

Local definitions

Linear partitioned control law for trajectory following

PartitionedControlLinear[xd_, dxd_, ddxd_, err_, derr_, kp_, kv_, 
          ki_, vals_] := 
        Module[{x, dx, α, β, fp}, 
x = 
            xd - err; 
dx = 
            dxd - derr; 
α = 
            m /. vals; 
β = 
            b dx + k x /. vals; 
fp = 
            ddxd + kv derr + kp err + 
              ki IntegralError;  
α fp  +  β
];

Linear PID control law (no model)

PIDLinear[xd_, dxd_, ddxd_, err_, derr_, kp_, kv_, 
          ki_] := 
PartitionedControlLinear[xd, dxd, ddxd, 
          err, derr, kp, kv, 
          ki, {m -> 1, b -> 0, k -> 0}];

Nonlinear partitioned control law for trajectory following

PartitionedControlNonlinear[xd_, dxd_, ddxd_, err_, derr_, kp_, kv_, 
          ki_, vals_] := 
        Module[{x, dx, α, β, fp}, 
x = 
            xd - err; 
dx = 
            dxd - derr; 
α = 
            m /. vals; 
β = 
            b dx + q x^3 /. vals; 
fp = 
            ddxd + kv derr + kp err + 
              ki IntegralError;  
α fp  +  β
];

Nonlinear PID control law (no model)

PIDNonlinear[xd_, dxd_, ddxd_, err_, derr_, kp_, kv_, 
          ki_] := 
PartitionedControlNonlinear[xd, dxd, 
          ddxd, err, derr, kp, kv, 
          ki, {m -> 1, b -> 0, q -> 0}];

PID trajectory following control (stable, linear system, no model)

Differential equation

Define simple second-order differential equation with input f

diffeq  =  
        m  Dt[x, {t, 2}]  +  b Dt[x, t]  +  k x  ==  
          f;

Display differential equation

diffeq  //  TraditionalForm

Numeric system values

system  =  {m -> 1,  b -> 1,  k -> 1};

Controller specification

Controller type

controlLaw  =  PIDLinear;

Controller frequency (Hz)

freq  =  50;

Simulation parameters

Total simulation time

tmax  =  15. ;

Desired trajectory (cubic interpolation)

path  =  {{0, 0}, {5, 4},  {7, 2}};

Create desired trajectory as interpolation function

desired  =  
        CreateDesiredTrajectory[path, {0, tmax, 1/freq}];

Control simulation #1 (small gains)

Control gain vectors

kp  =  9 //  N;
kv  =  6  //  N;
ki  =  0  //  N;

Control simulation

{actual, torques}  = 
        ControlAll[{diffeq /.  
              system},  {x},  {f},  {{path[[1, 2]]}, {0}},  tmax,  
          N[1/freq], {controlLaw, desired, kp, kv, ki}];

Visualization of results #1

Join trajectories

gdesired = 
        PlotMultipleTrajectory[desired[[1]], {0, tmax}, {Blue}, 
          nS];
gactual = 
        PlotMultipleTrajectory[actual, {0, tmax}, {Red}, 
          nS];
gtraj1  =  Show[gdesired, gactual, yS,  imgSize];

Error plot

error  =  
        CombineInterpolationFunctions[actual, desired[[1]], {0, tmax}, 
          Subtract];
gerror1 = 
        PlotMultipleTrajectory[error, {0, tmax}, {Green}, 
          imgSize];

Applied torques

gtorques1 = 
        PlotMultipleTrajectory[torques, {0, tmax}, {Magenta}, 
          imgSize];

Control simulation #2 (larger gains)

Control gain vectors

kp  =  25 //  N;
kv  =  10  //  N;
ki  =  0  //  N;

Control simulation

{actual, torques}  = 
        ControlAll[{diffeq /.  
              system},  {x},  {f},  {{path[[1, 2]]}, {0}},  tmax,  
          N[1/freq], {controlLaw, desired, kp, kv, ki}];

Visualization of results #2

Join trajectories

gdesired = 
        PlotMultipleTrajectory[desired[[1]], {0, tmax}, {Blue}, 
          nS];
gactual = 
        PlotMultipleTrajectory[actual, {0, tmax}, {Red}, 
          nS];
gtraj2  =  Show[gdesired, gactual, yS,  imgSize];

Error plot

error  =  
        CombineInterpolationFunctions[actual, desired[[1]], {0, tmax}, 
          Subtract];
gerror2 = 
        PlotMultipleTrajectory[error, {0, tmax}, {Green}, 
          imgSize];

Applied torques

gtorques2 = 
        PlotMultipleTrajectory[torques, {0, tmax}, {Magenta}, 
          imgSize];

Compare the two simulations

Show[
        GraphicsArray[{{gtraj1, gtraj2}, {gerror1, gerror2}, {gtorques1, 
              gtorques2}}],  ImageSize -> 500];

Control simulation #3 (even larger gains, integral gain as well)

Control gain vectors

kp  =  100 //  N;
kv  =  20  //  N;
ki  =  10  //  N;

Control simulation

{actual, torques}  = 
        ControlAll[{diffeq /.  
              system},  {x},  {f},  {{path[[1, 2]]}, {0}},  tmax,  
          N[1/freq], {controlLaw, desired, kp, kv, ki}];

Visualization of results #3

Join trajectories

gdesired = 
        PlotMultipleTrajectory[desired[[1]], {0, tmax}, {Blue}, 
          nS];
gactual = 
        PlotMultipleTrajectory[actual, {0, tmax}, {Red}, 
          nS];
gtraj3  =  Show[gdesired, gactual, yS,  imgSize];

Error plot

error  =  
        CombineInterpolationFunctions[actual, desired[[1]], {0, tmax}, 
          Subtract];
gerror3 = 
        PlotMultipleTrajectory[error, {0, tmax}, {Green}, 
          imgSize];

Applied torques

gtorques3 = 
        PlotMultipleTrajectory[torques, {0, tmax}, {Magenta}, 
          imgSize];

Compare the las two simulations

Show[
        GraphicsArray[{{gtraj2, gtraj3}, {gerror2, gerror3}, {gtorques2, 
              gtorques3}}],  ImageSize -> 500];

PID trajectory following control (nonlinear system, no model)

Differential equation

Define simple second-order differential equation with input f

diffeq  =  
        m  Dt[x, {t, 2}]  +  b Dt[x, t]  +  q x^3  ==  
          f;

Display differential equation

diffeq  //  TraditionalForm

Numeric system values

system  =  {m -> 1,  b -> 1,  q -> 1};

Controller specification

Controller type

controlLaw  =  PIDNonlinear;

Controller frequency (Hz)

freq  =  50;

Simulation parameters

Total simulation time

tmax  =  15. ;

Desired trajectory (cubic interpolation)

path  =  {{0, 0}, {5, 4},  {7, 2}};

Create desired trajectory as interpolation function

desired  =  
        CreateDesiredTrajectory[path, {0, tmax, 1/freq}];

Control simulation #1 (small gains)

Control gain vectors

kp  =  9 //  N;
kv  =  6  //  N;
ki  =  0  //  N;

Control simulation

{actual, torques}  = 
        ControlAll[{diffeq /.  
              system},  {x},  {f},  {{path[[1, 2]]}, {0}},  tmax,  
          N[1/freq], {controlLaw, desired, kp, kv, ki}];

Visualization of results #1

Join trajectories

gdesired = 
        PlotMultipleTrajectory[desired[[1]], {0, tmax}, {Blue}, 
          nS];
gactual = 
        PlotMultipleTrajectory[actual, {0, tmax}, {Red}, 
          nS];
gtraj1  =  Show[gdesired, gactual, yS,  imgSize];

Error plot

error  =  
        CombineInterpolationFunctions[actual, desired[[1]], {0, tmax}, 
          Subtract];
gerror1 = 
        PlotMultipleTrajectory[error, {0, tmax}, {Green}, 
          imgSize];

Applied torques

gtorques1 = 
        PlotMultipleTrajectory[torques, {0, tmax}, {Magenta}, 
          imgSize];

Control simulation #2 (larger gains)

Control gain vectors

kp  =  25 //  N;
kv  =  10  //  N;
ki  =  0  //  N;

Control simulation

{actual, torques}  = 
        ControlAll[{diffeq /.  
              system},  {x},  {f},  {{path[[1, 2]]}, {0}},  tmax,  
          N[1/freq], {controlLaw, desired, kp, kv, ki}];

Visualization of results #2

Join trajectories

gdesired = 
        PlotMultipleTrajectory[desired[[1]], {0, tmax}, {Blue}, 
          nS];
gactual = 
        PlotMultipleTrajectory[actual, {0, tmax}, {Red}, 
          nS];
gtraj2  =  Show[gdesired, gactual, yS,  imgSize];

Error plot

error  =  
        CombineInterpolationFunctions[actual, desired[[1]], {0, tmax}, 
          Subtract];
gerror2 = 
        PlotMultipleTrajectory[error, {0, tmax}, {Green}, 
          imgSize];

Applied torques

gtorques2 = 
        PlotMultipleTrajectory[torques, {0, tmax}, {Magenta}, 
          imgSize];

Compare the two simulations

Show[
        GraphicsArray[{{gtraj1, gtraj2}, {gerror1, gerror2}, {gtorques1, 
              gtorques2}}],  ImageSize -> 500];

Control simulation #3 (even larger gains, integral gain as well)

Control gain vectors

kp  =  100 //  N;
kv  =  20  //  N;
ki  =  10  //  N;

Control simulation

{actual, torques}  = 
        ControlAll[{diffeq /.  
              system},  {x},  {f},  {{path[[1, 2]]}, {0}},  tmax,  
          N[1/freq], {controlLaw, desired, kp, kv, ki}];

Visualization of results #3

Join trajectories

gdesired = 
        PlotMultipleTrajectory[desired[[1]], {0, tmax}, {Blue}, 
          nS];
gactual = 
        PlotMultipleTrajectory[actual, {0, tmax}, {Red}, 
          nS];
gtraj3  =  Show[gdesired, gactual, yS,  imgSize];

Error plot

error  =  
        CombineInterpolationFunctions[actual, desired[[1]], {0, tmax}, 
          Subtract];
gerror3 = 
        PlotMultipleTrajectory[error, {0, tmax}, {Green}, 
          imgSize];

Applied torques

gtorques3 = 
        PlotMultipleTrajectory[torques, {0, tmax}, {Magenta}, 
          imgSize];

Compare the las two simulations

Show[
        GraphicsArray[{{gtraj2, gtraj3}, {gerror2, gerror3}, {gtorques2, 
              gtorques3}}],  ImageSize -> 500];

Partitioned control  trajectory following (stable, linear system, perfect model)

Differential equation

Define simple second-order differential equation with input f

diffeq  =  
        m  Dt[x, {t, 2}]  +  b Dt[x, t]  +  k x  ==  
          f;

Display differential equation

diffeq  //  TraditionalForm

Numeric system values

system  =  {m -> 1,  b -> 1,  k -> 1};

Numerical model values

model  =  {m -> 1,  b -> 1,  k -> 1};

Controller specification

Controller type

controlLaw  =  PartitionedControlLinear;

Controller frequency (Hz)

freq  =  50;

Simulation parameters

Total simulation time

tmax  =  15. ;

Desired trajectory (cubic interpolation)

path  =  {{0, 0}, {5, 4},  {7, 2}};

Create desired trajectory as interpolation function

desired  =  
        CreateDesiredTrajectory[path, {0, tmax, 1/freq}];

Control simulation #1 (small gains)

Control gain vectors

kp  =  9 //  N;
kv  =  6  //  N;
ki  =  0  //  N;

Control simulation

{actual, torques}  = 
        ControlAll[{diffeq /.  
              system},  {x},  {f},  {{path[[1, 2]]}, {0}},  tmax,  
          N[1/freq], {controlLaw, desired, kp, kv, ki, model}];

Visualization of results #1

Join trajectories

gdesired = 
        PlotMultipleTrajectory[desired[[1]], {0, tmax}, {Blue}, 
          nS];
gactual = 
        PlotMultipleTrajectory[actual, {0, tmax}, {Red}, 
          nS];
gtraj1  =  Show[gdesired, gactual, yS,  imgSize];

Error plot

error  =  
        CombineInterpolationFunctions[actual, desired[[1]], {0, tmax}, 
          Subtract];
gerror1 = 
        PlotMultipleTrajectory[error, {0, tmax}, {Green}, 
          imgSize];

Applied torques

gtorques1 = 
        PlotMultipleTrajectory[torques, {0, tmax}, {Magenta}, 
          imgSize];

Control simulation #2 (larger gains)

Control gain vectors

kp  =  25 //  N;
kv  =  10  //  N;
ki  =  0  //  N;

Control simulation

{actual, torques}  = 
        ControlAll[{diffeq /.  
              system},  {x},  {f},  {{path[[1, 2]]}, {0}},  tmax,  
          N[1/freq], {controlLaw, desired, kp, kv, ki, model}];

Visualization of results #2

Join trajectories

gdesired = 
        PlotMultipleTrajectory[desired[[1]], {0, tmax}, {Blue}, 
          nS];
gactual = 
        PlotMultipleTrajectory[actual, {0, tmax}, {Red}, 
          nS];
gtraj2  =  Show[gdesired, gactual, yS,  imgSize];

Error plot

error  =  
        CombineInterpolationFunctions[actual, desired[[1]], {0, tmax}, 
          Subtract];
gerror2 = 
        PlotMultipleTrajectory[error, {0, tmax}, {Green}, 
          imgSize];

Applied torques

gtorques2 = 
        PlotMultipleTrajectory[torques, {0, tmax}, {Magenta}, 
          imgSize];

Compare the two simulations

Show[
        GraphicsArray[{{gtraj1, gtraj2}, {gerror1, gerror2}, {gtorques1, 
              gtorques2}}],  ImageSize -> 500];

Partitioned control  trajectory following (stable, linear system, imperfect model)

Differential equation

Define simple second-order differential equation with input f

diffeq  =  
        m  Dt[x, {t, 2}]  +  b Dt[x, t]  +  k x  ==  
          f;

Display differential equation

diffeq  //  TraditionalForm

Numeric system values

system  =  {m -> 1.1,  b -> 1.05,  k -> 0.95};

Numerical model values

model  =  {m -> 1,  b -> 1,  k -> 1};

Controller specification

Controller type

controlLaw  =  PartitionedControlLinear;

Controller frequency (Hz)

freq  =  50;

Simulation parameters

Total simulation time

tmax  =  15. ;

Desired trajectory (cubic interpolation)

path  =  {{0, 0}, {5, 4},  {7, 2}};

Create desired trajectory as interpolation function

desired  =  
        CreateDesiredTrajectory[path, {0, tmax, 1/freq}];

Control simulation #1 (small gains)

Control gain vectors

kp  =  9 //  N;
kv  =  6  //  N;
ki  =  0  //  N;

Control simulation

{actual, torques}  = 
        ControlAll[{diffeq /.  
              system},  {x},  {f},  {{path[[1, 2]]}, {0}},  tmax,  
          N[1/freq], {controlLaw, desired, kp, kv, ki, model}];

Visualization of results #1

Join trajectories

gdesired = 
        PlotMultipleTrajectory[desired[[1]], {0, tmax}, {Blue}, 
          nS];
gactual = 
        PlotMultipleTrajectory[actual, {0, tmax}, {Red}, 
          nS];
gtraj1  =  Show[gdesired, gactual, yS,  imgSize];

Error plot

error  =  
        CombineInterpolationFunctions[actual, desired[[1]], {0, tmax}, 
          Subtract];
gerror1 = 
        PlotMultipleTrajectory[error, {0, tmax}, {Green}, 
          imgSize];

Applied torques

gtorques1 = 
        PlotMultipleTrajectory[torques, {0, tmax}, {Magenta}, 
          imgSize];

Control simulation #2 (larger gains)

Control gain vectors

kp  =  25 //  N;
kv  =  10  //  N;
ki  =  0  //  N;

Control simulation

{actual, torques}  = 
        ControlAll[{diffeq /.  
              system},  {x},  {f},  {{path[[1, 2]]}, {0}},  tmax,  
          N[1/freq], {controlLaw, desired, kp, kv, ki, model}];

Visualization of results #2

Join trajectories

gdesired = 
        PlotMultipleTrajectory[desired[[1]], {0, tmax}, {Blue}, 
          nS];
gactual = 
        PlotMultipleTrajectory[actual, {0, tmax}, {Red}, 
          nS];
gtraj2  =  Show[gdesired, gactual, yS,  imgSize];

Error plot

error  =  
        CombineInterpolationFunctions[actual, desired[[1]], {0, tmax}, 
          Subtract];
gerror2 = 
        PlotMultipleTrajectory[error, {0, tmax}, {Green}, 
          imgSize];

Applied torques

gtorques2 = 
        PlotMultipleTrajectory[torques, {0, tmax}, {Magenta}, 
          imgSize];

Compare the two simulations

Show[
        GraphicsArray[{{gtraj1, gtraj2}, {gerror1, gerror2}, {gtorques1, 
              gtorques2}}],  ImageSize -> 500];

Control simulation #3 (integral gain as well)

Control gain vectors

kp  =  25 //  N;
kv  =  10  //  N;
ki  =  3  //  N;

Control simulation

{actual, torques}  = 
        ControlAll[{diffeq /.  
              system},  {x},  {f},  {{path[[1, 2]]}, {0}},  tmax,  
          N[1/freq], {controlLaw, desired, kp, kv, ki, model}];

Visualization of results #3

Join trajectories

gdesired = 
        PlotMultipleTrajectory[desired[[1]], {0, tmax}, {Blue}, 
          nS];
gactual = 
        PlotMultipleTrajectory[actual, {0, tmax}, {Red}, 
          nS];
gtraj3  =  Show[gdesired, gactual, yS,  imgSize];

Error plot

error  =  
        CombineInterpolationFunctions[actual, desired[[1]], {0, tmax}, 
          Subtract];
gerror3 = 
        PlotMultipleTrajectory[error, {0, tmax}, {Green}, 
          imgSize];

Applied torques

gtorques3 = 
        PlotMultipleTrajectory[torques, {0, tmax}, {Magenta}, 
          imgSize];

Compare the las two simulations

Show[
        GraphicsArray[{{gtraj2, gtraj3}, {gerror2, gerror3}, {gtorques2, 
              gtorques3}}],  ImageSize -> 500];

Partitioned control  trajectory following (nonlinear system, imperfect model)

Differential equation

Define simple second-order differential equation with input f

diffeq  =  
        m  Dt[x, {t, 2}]  +  b Dt[x, t]  +  q x^3  ==  
          f;

Display differential equation

diffeq  //  TraditionalForm

Numeric system values

system  =  {m -> 1.02,  b -> 1.01,  q -> 0.98};

Numerical model values

model  =  {m -> 1,  b -> 1,  q -> 1};

Controller specification

Controller type

controlLaw  =  PartitionedControlNonlinear;

Controller frequency (Hz)

freq  =  50;

Simulation parameters

Total simulation time

tmax  =  15. ;

Desired trajectory (cubic interpolation)

path  =  {{0, 0}, {5, 4},  {7, 2}};

Create desired trajectory as interpolation function

desired  =  
        CreateDesiredTrajectory[path, {0, tmax, 1/freq}];

Control simulation #1 (small gains, no integral term)

Control gain vectors

kp  =  9 //  N;
kv  =  6  //  N;
ki  =  0  //  N;

Control simulation

{actual, torques}  = 
        ControlAll[{diffeq /.  
              system},  {x},  {f},  {{path[[1, 2]]}, {0}},  tmax,  
          N[1/freq], {controlLaw, desired, kp, kv, ki, model}];

Visualization of results #1

Join trajectories

gdesired = 
        PlotMultipleTrajectory[desired[[1]], {0, tmax}, {Blue}, 
          nS];
gactual = 
        PlotMultipleTrajectory[actual, {0, tmax}, {Red}, 
          nS];
gtraj1  =  Show[gdesired, gactual, yS,  imgSize];

Error plot

error  =  
        CombineInterpolationFunctions[actual, desired[[1]], {0, tmax}, 
          Subtract];
gerror1 = 
        PlotMultipleTrajectory[error, {0, tmax}, {Green}, 
          imgSize];

Applied torques

gtorques1 = 
        PlotMultipleTrajectory[torques, {0, tmax}, {Magenta}, 
          imgSize];

Control simulation #2 (larger gains)

Control gain vectors

kp  =  25 //  N;
kv  =  10  //  N;
ki  =  0  //  N;

Control simulation

{actual, torques}  = 
        ControlAll[{diffeq /.  
              system},  {x},  {f},  {{path[[1, 2]]}, {0}},  tmax,  
          N[1/freq], {controlLaw, desired, kp, kv, ki, model}];

Visualization of results #2

Join trajectories

gdesired = 
        PlotMultipleTrajectory[desired[[1]], {0, tmax}, {Blue}, 
          nS];
gactual = 
        PlotMultipleTrajectory[actual, {0, tmax}, {Red}, 
          nS];
gtraj2  =  Show[gdesired, gactual, yS,  imgSize];

Error plot

error  =  
        CombineInterpolationFunctions[actual, desired[[1]], {0, tmax}, 
          Subtract];
gerror2 = 
        PlotMultipleTrajectory[error, {0, tmax}, {Green}, 
          imgSize];

Applied torques

gtorques2 = 
        PlotMultipleTrajectory[torques, {0, tmax}, {Magenta}, 
          imgSize];

Compare previous two simulations

Show[
        GraphicsArray[{{gtraj1, gtraj2}, {gerror1, gerror2}, {gtorques1, 
              gtorques2}}],  ImageSize -> 500];

Control simulation #3 (even larger gains, integral gain as well)

Control gain vectors

kp  =  100 //  N;
kv  =  20  //  N;
ki  =  10  //  N;

Control simulation

{actual, torques}  = 
        ControlAll[{diffeq /.  
              system},  {x},  {f},  {{path[[1, 2]]}, {0}},  tmax,  
          N[1/freq], {controlLaw, desired, kp, kv, ki, model}];

Visualization of results #3

Join trajectories

gdesired = 
        PlotMultipleTrajectory[desired[[1]], {0, tmax}, {Blue}, 
          nS];
gactual = 
        PlotMultipleTrajectory[actual, {0, tmax}, {Red}, 
          nS];
gtraj3  =  Show[gdesired, gactual, yS,  imgSize];

Error plot

error  =  
        CombineInterpolationFunctions[actual, desired[[1]], {0, tmax}, 
          Subtract];
gerror3 = 
        PlotMultipleTrajectory[error, {0, tmax}, {Green}, 
          imgSize];

Applied torques

gtorques3 = 
        PlotMultipleTrajectory[torques, {0, tmax}, {Magenta}, 
          imgSize];

Compare the las two simulations

Show[
        GraphicsArray[{{gtraj2, gtraj3}, {gerror2, gerror3}, {gtorques2, 
              gtorques3}}],  ImageSize -> 500];