% 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]';