% 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,:,:))])
%