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

% Parameters
% -------------------------------------------------------------------------
l = 0.2; % Distance between wheels
r = 0.05; % Radius of the wheels (only for display purposes)
v1 = 0.1; % Speed of the rear wheel
v2 = -0.1; % Speed of the rear wheel
v3 = 0.1; % 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.
% EXAMPLE
theta = 0 *pi/180;
x = 0;
y = 0;
% In this case the robot is initially placed at <0,0> at an angle of 0
% degrees.

% Geometrical parameters
% -------------------------------------------------------------------------
vehicle.wheels(1).alpha = 0;
vehicle.wheels(1).beta = 0;
vehicle.wheels(1).gamma = 0;
vehicle.wheels(1).l = l;
vehicle.wheels(1).r = r;
vehicle.wheels(2).alpha = -pi*2/3;
vehicle.wheels(2).beta = 0;
vehicle.wheels(2).gamma = 0;
vehicle.wheels(2).l = l;
vehicle.wheels(2).r = r;
vehicle.wheels(3).alpha = +pi*2/3;
vehicle.wheels(3).beta = 0;
vehicle.wheels(3).gamma = 0;
vehicle.wheels(3).l = l;
vehicle.wheels(3).r = r;

% 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 = 5;
t = 0;

% Generate variables
% -------------------------------------------------------------------------
for ii = 1:numel(vehicle.wheels)
    eval(sprintf('alpha%i = vehicle.wheels(ii).alpha;',ii));
    eval(sprintf('beta%i = vehicle.wheels(ii).beta;',ii));
    eval(sprintf('gamma%i = vehicle.wheels(ii).gamma;',ii));
    eval(sprintf('l%i = vehicle.wheels(ii).l;',ii));
end

% 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
    % ---------------------------------------------------------------------
    % Constraints matrix
    J1 = [sin(alpha1 + beta1 + gamma1), -cos(alpha1 + beta1 + gamma1), -l1*cos(beta1 + gamma1) ; ...
          sin(alpha2 + beta2 + gamma2), -cos(alpha2 + beta2 + gamma2), -l2*cos(beta2 + gamma2) ; ...
          sin(alpha3 + beta3 + gamma3), -cos(alpha3 + beta3 + gamma3), -l3*cos(beta3 + gamma3) ];
    C1 = [cos(alpha1 + beta1 + gamma1),  sin(alpha1 + beta1 + gamma1),  l1*sin(beta1 + gamma1) ; ...
          cos(alpha2 + beta2 + gamma2),  sin(alpha2 + beta2 + gamma2),  l2*sin(beta2 + gamma2) ; ...
          cos(alpha3 + beta3 + gamma3),  sin(alpha3 + beta3 + gamma3),  l3*sin(beta3 + gamma3) ];
    % Wheel speed matrix J2
    J2 = [ ...
        cos(gamma1) * v1 ; ...
        cos(gamma2) * v2 ; ...
        cos(gamma3) * v3 ];
    % 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;
    
    % Save in array
    % ---------------------------------------------------------------------
    xs(ii) = x;
    ys(ii) = y;
    
    % Plot
    % ---------------------------------------------------------------------
    cla
    plotRobot(x,y,theta,vehicle)
    xlim([-1 1])
    ylim([-1 1])
    daspect([1 1 1])
    drawnow
end

% Plot trajectory
plot(xs,ys,'r')



function plotRobot(x,y,theta,vehicle)
    % This function plots the vehicle geometry based on the data in vehicle
    nWheels = numel(vehicle.wheels);
    w = zeros(nWheels,2);
    w1 = zeros(nWheels,2);
    w2 = zeros(nWheels,2);
    for ii = 1:nWheels
        alpha = vehicle.wheels(ii).alpha;
        beta = vehicle.wheels(ii).beta;
        w(ii,1) = x + vehicle.wheels(ii).l * cos(theta + alpha);
        w(ii,2) = y + vehicle.wheels(ii).l * sin(theta + alpha);
        w1(ii,1) = w(ii,1) + vehicle.wheels(ii).r * cos(theta + alpha + beta + pi/2);
        w1(ii,2) = w(ii,2) + vehicle.wheels(ii).r * sin(theta + alpha + beta + pi/2);
        w2(ii,1) = w(ii,1) - vehicle.wheels(ii).r * cos(theta + alpha + beta + pi/2);
        w2(ii,2) = w(ii,2) - vehicle.wheels(ii).r * sin(theta + alpha + beta + pi/2);
    end
    
    % Plot
    plot([x x+0.2*cos(theta)],[y y+0.2*sin(theta)],'b')
    hold on
    plot(x,y,'bo')
    for ii = 1:nWheels
        plot([w1(ii,1) w2(ii,1)],[w1(ii,2) w2(ii,2)], 'lineWidth', 2,'color','k');
    end
end