% Bicycle drive robot: forward kinematics
% =========================================================================
% In this exercise we can see how a steered bicycle robot behaves based on
% the speed of its wheels.
clc
clear all
close all

% Parameters
% -------------------------------------------------------------------------
wheel_radius = 0.05; % Radius of the wheels (only for display purposes)
% EXAMPLE 1
l = 0.2; % Distance between wheels
v = 0.05; % Speed of the rear wheel
% EXAMPLE 2
% l = 0.4; % Distance between wheels
% v = 0.05; % Speed of the rear wheel

% Initial conditions
% -------------------------------------------------------------------------
% < x , y > defines the position of the robot in the global inertial frame
% of reference < X_I , Y_I >.
% The orientation of the robot is defined by the angle theta with respect
% to the x axis of the global inertial frame of reference.
% The angle phi determines the angular coordinate of the steering axis,
% while dphi is its rotation speed.
% EXAMPLE 1
% phi = -80 *pi/180;
% dphi = 0.08;
% theta = 45 *pi/180;
% x = 0;
% y = 0;
% In this case the robot is initially placed at <0,0> at an angle of 45
% degrees, the steering axis is turned to the right by 80 degrees, and its
% speed is 0.08 rad/s counter-clockwise.
% EXAMPLE 2
phi = 30 *pi/180;
dphi = 0*0.08;
theta = 0 *pi/180;
x = 0;
y = 0;
% In this case the robot is initially placed at <0,0> at an angle of 45
% degrees, the steering axis is turned to the right by 80 degrees, and its
% speed is 0.08 rad/s counter-clockwise.

% Geometrical parameters
% -------------------------------------------------------------------------
alpha1 = 0;
beta1 = pi/2;
l1 = 0;
alpha2 = 0;
l2 = l;

% Setup time integration
% -------------------------------------------------------------------------
% Here are defined the parameters which concern the time-integrated
% simulation, namely the initial time t, the timestep dt and the end time
% t_end.
dt = 0.1;
% t_end = 33; % EXAMPLE 1
t_end = 45; % EXAMPLE 2
t = 0;

% Create figure
% -------------------------------------------------------------------------
hold on
xlabel('$X_I [m]$','Interpreter','latex')
ylabel('$Y_I [m]$','Interpreter','latex')
%set(gcf,'WindowStyle','docked')

% Initialize variables and arrays
t_array = t:dt:t_end;
nSteps = numel(t_array);
% Initialize results array
xs = zeros(nSteps,1);
ys = zeros(nSteps,1);
x2s = zeros(nSteps,1);
y2s = zeros(nSteps,1);
for ii = 1:nSteps
    % Kinematics model
    % ---------------------------------------------------------------------
    beta2 = -pi/2 + phi;
    v1 = v;
    % Constraints matrix
    J1 = [cos(alpha1 + beta1),  sin(alpha1 + beta1),  l1*sin(beta1) ; ...
          sin(alpha1 + beta1), -cos(alpha1 + beta1), -l1*cos(beta1) ; ...
          cos(alpha2 + beta2),  sin(alpha2 + beta2),  l2*sin(beta2) ];
    % Wheel speed matrix J2
    J2 = [0 ; v1 ; 0];
    % Inverse rotation matrix
    R_inv = [cos(theta) -sin(theta) 0 ; ...
             sin(theta)  cos(theta) 0 ; ...
             0           0          1 ];
	% Solve the system
    dq = R_inv * inv(J1) * J2;
    
    % Get results in a more readable format
    dx = dq(1);
    dy = dq(2);
    dtheta = dq(3);
    
    % Time integration
    % ---------------------------------------------------------------------
    x = x + dx*dt;
    y = y + dy*dt;
    theta = theta + dtheta*dt;
    phi = phi + dphi*dt;
    
    % Calculate other points
    % ---------------------------------------------------------------------
    x2 = x + l*cos(theta); % Steering axis x-coordinate
    y2 = y + l*sin(theta); % Steering axis y-coordinate
    x3r = x - wheel_radius*cos(theta); % Rear wheel tip x-coordinate (1)
    y3r = y - wheel_radius*sin(theta); % Rear wheel tip y-coordinate (1)
    x4r = x + wheel_radius*cos(theta); % Rear wheel tip x-coordinate (2)
    y4r = y + wheel_radius*sin(theta); % Rear wheel tip y-coordinate (2)
    x3f = x2 - wheel_radius*cos(theta + phi); % Front wheel tip x-coordinate (1)
    y3f = y2 - wheel_radius*sin(theta + phi); % Front wheel tip y-coordinate (1)
    x4f = x2 + wheel_radius*cos(theta + phi); % Front wheel tip x-coordinate (2)
    y4f = y2 + wheel_radius*sin(theta + phi); % Front wheel tip y-coordinate (2)
    
    % Save in array
    % ---------------------------------------------------------------------
    xs(ii) = x;
    ys(ii) = y;
    x2s(ii) = x2;
    y2s(ii) = y2;
    
    % Plot
    % ---------------------------------------------------------------------
    cla
    plot([x x2],[y y2]); % Rover body
    hold on;
    plot([x3r x4r],[y3r y4r], 'lineWidth', 2,'color','k'); % Rear wheel
    plot([x3f x4f],[y3f y4f], 'lineWidth', 2,'color','k'); % Front wheel
    plot(x4f,y4f, '.', 'markersize', 14,'color','r'); % Front wheel marker
    plot(xs(1:ii),ys(1:ii)); % Rear wheel trajectory
    plot(x2s(1:ii),y2s(1:ii)); % Steering axis trajectory
%     xlim([-1 0.4]) % EXAMPLE 1
%     ylim([-0.4 1])
    xlim([-1 2]) % EXAMPLE 2
    ylim([-2 1])
    daspect([1 1 1])
    drawnow
end