% Skid-Steer Mobile Manipulator Model and Simulation Using the
% Spatial Toolbox for Rigid Body Dynamics
% 
% Copyright (c) 2014.02.01 
%   Sergio Aguilera-Marinovic (sergio.aguilera.marinovic@gmail.com)
%   Miguel Torres-Torriti     (mtorrest@ing.puc.cl)
%   Robotics and Automation Laboratory
%   Pontificia Universidad Catolica de Chile
%   http://ral.ing.puc.cl/ssmm.htm
% 
% Version 1.0 - 2014.02.01
%
% Description
%  This Matlab script shows how to build the model for an SSMM using the 
%  Spatial Toolbox for rigid body dynamics modeling developed by Roy
%  Featherstone available at http://royfeatherstone.org.
%
%  The model considers a floating base with four wheels and a simplfied
%  1-DOF arm.  It is possible to easily add more degrees of freedom to the
%  arm by copying the data structure for the 1-DOF arm and updating the
%  parent link data where appropriate.  The reason of implementing 1-DOF is
%  to keep the code as simple and illustrative as possible, but more
%  important, to replicate the dynamics of a Caterpillar CAT262C skid-steer
%  compact loader.
%
%  The model simulation results have been compared with IMU measuremnts
%  obtained from experiments with a real CAT262C machine.  The data from
%  the experiments is available at http://ral.ing.puc.cl/ssmm.htm.
%
%
% Instructions
%  1. Prerequisites in addition to a standard installation of Matlab and 
%     Simulink is two download and setup the Spatial Toolbox version 2 by 
%     Roy Featherstone available at http://royfeatherstone.org.
%
%  2. Initialize the Spatial Toolbox with the command 'startup.m', which
%     adds its installation path to Matlab's environment list of paths.
%
%  3. Change the directory to the location of the SSMM script files 
%     (this model and simulation files).
% 
%  4. Run the SSMM_model.m file, it will create and store the SSMM model
%     in variable 'model'.
%
%  5. Open the SSMM_examples.m script, uncomment one of the four example
%     cases, save the file and run it from Matlab's command line
%     interface.
%
%  6. Run the Simulink simulation file SSMM_sim.slx.  The output of the 
%     simulation is stored in the variable xout, which is a 23x1xN
%     array structure.  To reduce the singleton dimension you may execute
%     res = squeeze(xout), which will store the results in variable res
%     23xN array.  
% 
%     The 23 model state variables are the following:
%     1. The floating base varaibles:
%
%       x = [x1 x2 x3 x4 x5 x6 x7 x8 x9 x10 x11 x12 x13]';
%            |_________| |______| |_______| |_________|
%                 |          |        |          |->Linear Velocity in
%                 |          |        |               F1 Coordinates
%                 |          |        |
%                 |          |        |->Angular Velocity in
%                 |          |             F1 Coordinates
%                 |          |        
%                 |          |->Position relative to F_0
%                 |
%                 |->Orientation Quaternion
%
%     2. The 1-DOF joint positions:
%
%       q = [q1 q2 q3 q4 q5]'
%             |________|  |
%                 |       |->Arm joint position
%                 |
%                 |->Wheel joint positions
%
%     3. The 1-DOF joint velocities:
%
%       qd = [qd1 qd2 qd3 qd4 qd5]'
%             |_____________|  |
%                    |         |->Arm joint velocity
%                    |
%                    |->Wheel joint velocities
%
%     The full state vector with results of the simulation is stored in
%     the variable xout and contains the previous vectors, as follows:
%
%       xout = [x q qd]'
%
%  7. At the end of the simulation you may execute:
%     res = squeeze(xout)
%     plot(tout,res(11,:)) % plots the longitudinal velocity of the mobile
%                          % base
%     plot(tout,res(5,:))  % plots the longitudinal displacement of the
%                          % mobile base
%     plot(tout,res(18,:)) % plots the arm position
% 
%     The following command renders a 3D representation of the 
%     model and its motion using showmotion provided with the Spatial
%     Toolbox:
%    
%     showmotion(model,tout,[fbanim(xout);squeeze(xout(14:18,:,:))])
%

% ============================ SSMM Model ============================

% The model script stars here...
%
clear all; clc; % Clear the workspace and erase the command window.

%--------------------- Reference Frame F0 ----------------------
% Draw the reference frame F_0 axes X_0, Y_0 and Z_0 using the "appearance"
% attribute to provide a visual reference in space
model.appearance.base = ...
  { 'colour', [0.9 0 0], 'line', [0 0 0; 2 0 0], ...
    'colour', [0 0.9 0], 'line', [0 0 0; 0 2 0], ...
    'colour', [0 0.3 0.9], 'line', [0 0 0; 0 0 2] };


%--------------------- SSMM Model ----------------------
%
% Store all model parameters in a variable called "model".
% model.NB: is the number of bodies in the model.
% Variable model.NB is initialized in zero and incremented whenever a new
% body is added to the model...  This allows to easily expand any model!
model.NB = 0;


%---------------------Floating Base----------------------
%
% To create a floting base, a body (with frame F1) is added and "connected"
% to the reference frame F0 by any 1-DOF joint (e.g. rotary or prismatic),
% which will be later replaced by a full 6-DOF joint using the function 
% floatbase.  The 1-DOF joint is simply a temporary placeholder for the
% 6-DOF joint.
i=1; % This is the first body, and it's index is one.
model.NB=model.NB+1; % model.NB is updated with the new body.

model.jtype{i}  = 'R'; % Any type of joint may be selected

model.parent(i) =  0 ; % The floating base parent is the fixed frame, 
                       % i.e. \lambda(1) = 0

% The initial link-to-link transform from F_0 to F_1 is the identity,
% because at the initial state both frames are align.
model.Xtree{i}  = xlt([0 0 0]); 

mass(i)         = 1150;  % The mass of the CAT 262C is for about 1150 kg

% For modeling simplicity, the center of mass (COM) of the base is defined
% at the origin of frame F1.
CoM(i,:)        = [0 0 0]; 

% The rotational inertia for the SSMM is approximated to the rotational
% inertia of a cube of size [3 1.6 1.2] with uniform density and total mass
% equal to mass(i).
Icm(i:i+2,:)    = mass(i)*[1.6^2+1.2^2     0            0;...
                            0           3^2+1.2^2       0;...
                            0              0         3^2+1.6^2]/12;
                        
% The mass, COM and Icm are employed to build the rigid-body's spatial 
% inertia, which is stored in the model parameter "model.I{1}".
model.I{i}      = mcI(mass(i), CoM(i,:), Icm(i:i+2,:));

% Define the floating base appearance to display/visualize the simulation
% results.
model.appearance.body{1} = {'colour', [250 137 45]/255, 'box',   [-1.2 -0.8 -0.6; 1.8 0.8 0]...
                            'colour', [250 137 45]/255, 'box',   [-1.2  -0.8 0; -0.3 0.8 0.6]... 
                            'colour', [0.5 0.5 0.5], 'box', [  0  -0.7  0; 1.7 0.7 0.85] };


%--------------------- Wheels ----------------------
% Wheels are the bodies 2, 3, 4 and 5.
for i = 2:5
model.NB=model.NB+1; % Update the body number for each wheel

% Wheels are modeled as rotary joints that rotate about the Y-axis of
% frames F2, F3, F4, and F5 (children of parent frame F1).
model.jtype{i} = 'Ry'; 

% The wheels' parent body is the floating base, i.e. \lambda(i) = 1
model.parent(i) = 1 ;

R = 0.45; % The wheels radius is .45 m
T = 0.25; % The thickness of the wheels is .25 m
density = 300; % THe density of each wheel is 300 kg/m^3

% As shown in the paper, each wheel has a different location and the 
% link-to-link transform is only a translation of the wheel frame with
% to the body frame without rotation.
if(i==2)model.Xtree{i} = xlt([  1   -0.8+T/4  -0.5]);end
if(i==3)model.Xtree{i} = xlt([-0.2  -0.8+T/4  -0.5]);end
if(i==4)model.Xtree{i} = xlt([  1    0.8-T/4  -0.5]);end
if(i==5)model.Xtree{i} = xlt([-0.2   0.8-T/4  -0.5]);end


% The mass of the wheels equals to the volumen of the wheel times the
% density
mass(i) = pi*R^2*T*density;

% Because of the wheel clindrical shape the COM is located at the origin
% of their frame which also coincides with the geometrical centroid of the
% wheel.
CoM(i,:)        = [0 0 0]; 

% The wheels are modeled as solid cylinders of radius of .45 m and 
% thickness of .25 m for the purpose of calculating and approximation to 
% their rotational inertia.
Ibig = mass(i)*R^2/2;
Ismall = mass(i)*(3*R^2 +T^2)/12;

% The spatial rigid-body inertia is calculated for each wheel using the
% mass, COM and inertia tensor.
model.I{i} = mcI(mass(i),CoM(i,:),diag([Ismall Ibig Ismall])); 

% Define the wheels' visual representation attributes.
model.appearance.body{i} = { 'colour', [0.1 0.1 0.1],...
                             'facets', 32,...
                             'cyl', [0 -T/2 0; 0 T/2 0], R };
end


%--------------------- Manipulator ----------------------
% This is a 1-DOF simplified arm, so only 1 joint will be defined.
% However, the next block of code can be copied to create additional joints
% and add DOFs as needed.

i=6; % The arm is the body number six.

model.NB=model.NB+1; % The model's body count is updated.

% For the CAT 262C skid-steer loader bucket arm movement is simplfied in
% this model to a rotary joint that rotates about the Y-axis of frame F6.
% While the actual machine has hydraulic (linear) pistons that actuate on
% the bucket arm, the pistons are hinged and the arm is in fact attached 
% to rotary joint on a hinged mechanism with multiple bars.  This mechanism
% allows the pivoting point to move a little forward/backward when the
% the arm is fully up/down.  This feature has been left out of the model
% to keep it simple and pedagogical, but can be modeled defining the
% additional linking bodies and passive (i.e. non-actuated) joints to
% create a closed-chain type of four-bar mechanism.
model.jtype{i}  = 'Ry'; 

% The manipulator parent is also the floating base, so the sixth element of 
% the parent array is set to 1 (the floating base), i.e. \lambda(6) = 1.
model.parent(i) =  1;

% The link-to-link transform for the manipulator arm corresponds to a
% translation of frame F6 by [-1.2 0 0.6] relative to frame F1.  This
% translation positions the arm at the top-rear of the CAT 262C. 
% The initial joint position with q6=0 is defined to be the position where 
% the arm's end-effector (bucket) is almost touching the ground.  
% To this end, a 15° rotation about the Y-axes of F6 is applied.
model.Xtree{i}  = roty(15*pi/180)*xlt([-1.2 0 0.6]);

l_arm(i)        =  3.3;  % The arm's length
mass(i)         =  100;  % The arm's mass
r               =  0.15; % The arm is modeled as a cylinder of radius r

% The center of mass of the arm is located at the mid-point of the arm's
% length.
CoM(i,:)        = l_arm(i)*[0.5,0,0];

% The inertia matrix of the arm is approximated to that of a solid cylinder
% (like in the case of the wheels), and is given by
Icm(i+3:i+5,:)  = mass(i)*[ r^2*6           0                0;...
                             0      r^2*3+l_arm(i)^2         0;...
                             0              0           r^2*3+l_arm(i)^2]/12;

% The spatial rigid-body inertia of the arm link is calculated using the
% mass, the COM and the inertia matrix:
model.I{i}      = mcI(mass(i), CoM(i,:), Icm(i+3:i+5,:));

% Finally, define the arm's visual representation attributes to ressemble
% that of a compact skid-steer loader.
rotation = roty(-15*pi/180);
rot = rotation(1:3,1:3);
model.appearance.body{i} = ...
      { %'cyl', [0 0 0; 3.3 0 0], 0.15...
        'colour', [250 137 45]/255,...
        'cyl', [0 0.9 0; 1.3 0.9 0]*rot, 0.15, ...
        'cyl', [1.2 0.9 0; 3.25 0.9 -0.6]*rot, 0.15, ...
        'cyl', [3.15 0.9 -0.5; 3.15 0.9 -0.9]*rot, 0.15, ...
        ...
        'cyl', [0 -0.9 0; 1.3 -0.9 0]*rot, 0.15, ...
        'cyl', [1.2 -0.9 0; 3.25 -0.9 -0.6]*rot, 0.15, ...
        'cyl', [3.15 -0.9 -0.5; 3.15 -0.9 -0.9]*rot, 0.15, ...
        ...
        'cyl', [0 -1.05 0; 0 1.05 0]*rot, 0.18, ...
        'colour', [0.1 0.1 0.1],...
        'cyl', [3.15 -1 -0.9; 3.15 1 -0.9]*rot, 0.1,...
        'box', [3.15 -1.1 -0.95;3.6  1.1 -0.85]*rot};

% To see the idealized (cylindrical) model arm, instead of an arm that 
% visually ressembles that of the compact skid-steer loader, comment the 
% previous appearance attributes and uncomment the folowing line:
% model.appearance.body{i} = { 'cyl', [0 0 0; 3.3 0 0], 0.15};

    
%--------------------- Additional Model Parameters ----------------------
% The default gravity is zero, so it must be defined as:
model.gravity = [0 0 -9.8];

% Once each body in the model has been defined, the first body must be 
% turned into a floating base:
model = floatbase(model);
% By doing this, the body 1 has now 6 DOFs and can be tought as if it would
% be formed by the composition of 6 bodies each having a 1-DOF joint.  So
% the first six joint variables belong to body 1, while the wheels which
% to be associated to bodies/frames 2, 3, 4, 5 are now bodies/frames 7, 8,
% 9, 10.  Similarly, the arm body 6 is now boy 11, and the model variable
% model.NB is now valued model.NB+5.

%--------------------- Contact Points (CPs) ----------------------
% The contact points are defined as point that cannot penetrate the ground
% plane defined as the plane z=0 in the frame F0.  Each contact point
% contains information about the body to which it belongs and its location
% in the body's reference frame.

% Floating Base CPs --------------------------------------------
CP_Base =[-1.2  1.8 -1.2  1.8 -1.2  1.8 -1.2  1.8;... X parameter of each CP
          -0.8 -0.8  0.8  0.8 -0.8 -0.8  0.8  0.8;... Y parameter of each CP
          -0.6 -0.6 -0.6 -0.6  0.6  0.6  0.6  0.6];  %Z parameter of each CP
                   
% The body number of each CP
CP_Base_Body_Labels = 6*ones(1,length(CP_Base));

% Total number of CPs for the floating base
CP_Base_Num = length(CP_Base_Body_Labels);


% Wheels' CPs---------------------------------------------------
% Because of the wheels' simmetry, all CPs are located equidistant one
% from another about the perimeter of each wheel.
npt_1 = 32; % 32 CPs per wheel are been modeled

% The position for each CP on a wheel is calculated next.
ang = (0:npt_1-1) * 2*pi / npt_1;
Y = ones(1,npt_1) * T/2;
X = sin(ang) * R;
Z = cos(ang) * R;

CP_Wheel = [ X;...
             Y;...
             Z ];
          
% Define the corresponding body for the wheels' CPs
CP_Wheel_1_Body_Labels =  7*ones(1,length(CP_Wheel(1,:)));
CP_Wheel_2_Body_Labels =  8*ones(1,length(CP_Wheel(1,:)));
CP_Wheel_3_Body_Labels =  9*ones(1,length(CP_Wheel(1,:)));
CP_Wheel_4_Body_Labels = 10*ones(1,length(CP_Wheel(1,:)));

% The CPs of all four wheels are store in a single variable:
CP_Wheels = [CP_Wheel CP_Wheel CP_Wheel CP_Wheel];

% All the corresponding body label for the wheels' CPs are also stored
% in a single variable:
%Wheels_Parent = [Cuerpo_wheel_1 Cuerpo_wheel_2 Cuerpo_wheel_3 Cuerpo_wheel_4];
CP_Wheels_Body_Labels = [CP_Wheel_1_Body_Labels CP_Wheel_2_Body_Labels...
                         CP_Wheel_3_Body_Labels CP_Wheel_4_Body_Labels];

% Total number of wheel contact points
CP_Wheels_Num = length(CP_Wheel_1_Body_Labels)*4;


% Manipulator CP -----------------------------------------------
% Only one CP is defined at end of the arm where the end-effector (bucket
% of the skid-steer loader) is located 
CP_Arm = [3.3;...
           0;...
           0 ];
        
% The arm contact point belogs to body 11
%Arm_Parent
CP_Arm_Body_Labels = 11*ones(1,length(CP_Arm(1,:)));

% Total number of arm contact points
CP_Arm_Num = length(CP_Arm_Body_Labels);


% External Forces --------------------------------------------
% The engine force is being modeled as external forces applied to the
% wheel-base joint. For this, a auxiliar contact point is created as 
% before:
CP_Fext = [   1 -0.2    1 -0.2;...
           -0.8 -0.8  0.8  0.8;...
           -0.5 -0.5 -0.8  -0.5];

Fext_straight = [625 625 625 625;0 0 0 0; 0 0 0 0];
Fext_turn = [1625 1625 1625 1625;0 0 0 0; 0 0 0 0];

CP_Fext_Body_Labels = 6*ones(1,length(CP_Fext(1,:)));
CP_Fext_Num = length(CP_Fext_Body_Labels);


%------------------------- Model Format ------------------------
% All the contact points and parents position previously defined
% are put on the Spatial Toolbox format as shown below
model.gc.point = [CP_Base CP_Wheels CP_Arm CP_Fext];
model.gc.body = [CP_Base_Body_Labels CP_Wheels_Body_Labels CP_Arm_Body_Labels CP_Fext_Body_Labels];

% The simulation in Simulink needs some auxiliar variables that define 
% the starting index of the wheels's CPs within the general CP array 
% (model.gc.point) for each wheel separately.
CP_Wheel_1_Index = length(CP_Base_Body_Labels)+1;
CP_Wheel_2_Index = length(CP_Base_Body_Labels)+length(CP_Wheels_Body_Labels)/4+1;
CP_Wheel_3_Index = length(CP_Base_Body_Labels)+length(CP_Wheels_Body_Labels)*2/4+1;
CP_Wheel_4_Index = length(CP_Base_Body_Labels)+length(CP_Wheels_Body_Labels)*3/4+1;
CP_Wheels_Final_Index = length(CP_Base_Body_Labels)+length(CP_Wheels_Body_Labels)*4/4;

% Auxiliar variables are declared to store the total number of CPs in the
% simulation considering the external forces and without including the
% external forces:
CP_Num = CP_Base_Num + CP_Arm_Num + CP_Wheels_Num + CP_Fext_Num;
CP_Num_aux = CP_Base_Num + CP_Arm_Num + CP_Wheels_Num; 



%-------------------- Initialization --------------------------------
% Finally, the initial condition is declared:
x_init = [1 0 0 0  0 0 0.95   0 0 0  0 0 0]';
%         |______| |_______| |_____| |____|
%             |        |        |       |->Linear Velocity in F_1
%             |        |        |          Coordinates
%             |        |        |
%             |        |        |->Angular Velocity in F_1 coordinates
%             |        |        
%             |        |->Position relative to F_0
%             |
%             |->Orientation Quaternion
%
% q_init(1:4) contains the wheels' initial position and q_init(5) contains
% the arm's initial position
q_init = [0 0 0 0 -0.2541]';

% qd_init(1:4) contains the wheels' initial velocities and q_init(5) contains
% the arm's initial velocity
qd_init = [0 0 0 0 0]';
