
%
% Chapter 3, Example 3.1, pages 70-73
% Optimal Reference Shaping for Dynamical Systems
% Theory and Applications
% -Tarunraj Singh
%
function Example_31

x0 = [1.07 1.2];
xopt = fsolve(@brachsol,x0,[],1,1);
sigma =xopt(2);
kappa =xopt(1);
sigmavec = linspace(0,sigma);
x = kappa^2*(sigmavec-0.5*sin(2*sigmavec));
y = 0.5*kappa^2*(1-cos(2*sigmavec));
plot(x,-y);
xlabel('X');
ylabel('Y');
hold on

x0 = [1.07 1.2];
xopt = fsolve(@brachsol,x0,[],2,1);
sigma =xopt(2);
kappa =xopt(1);
sigmavec = linspace(0,sigma);
x = kappa^2*(sigmavec-0.5*sin(2*sigmavec));
y = 0.5*kappa^2*(1-cos(2*sigmavec));
plot(x,-y,'r--');

x0 = [1.07 1.2];
xopt = fsolve(@brachsol,x0,[],3,1);
sigma =xopt(2);
kappa =xopt(1);
sigmavec = linspace(0,sigma);
x = kappa^2*(sigmavec-0.5*sin(2*sigmavec));
y = 0.5*kappa^2*(1-cos(2*sigmavec));
plot(x,-y,'g-.');

function f = brachsol(x,xf,yf)
kappa = x(1);
sigma = x(2);

% Code to use fsolve to determine kappa and max of sigma to reach the final
% position of x = xf, and y = yf
f(1) = 1/2*kappa^2*(1-cos(2*sigma))-yf; % y component
f(2) = kappa^2*(sigma-1/2*sin(2*sigma))-xf; % x component
