Zero+Moment+Point+Tutorial+1+-+Create+a+Simple+Robot

= = toc This document is an adaption of this Matlab script which can be downloaded and run in Matlab:

This file uses save data for the joint coordinates:

=The robot=

The robot is a mobile robot platform with a straight pole on top. In this first simulation, the pole is rigidly attached to the mobile platform. The measurements of the robot are:
 * ~ Parameter __ ||~ Value ||
 * L1 || 0.15 ||
 * L2 || 0.05 ||
 * L3 || 0.1 ||
 * h1 || 0.05 ||
 * h2 || 0.5 ||
 * m1 || 2 ||
 * m2 || 6 ||

=Define Robot measurements= code format="matlab" L1 = 0.15;                 % Distance between rear wheel and CoM of Robot Platform L2 = 0.05;                 % Distance between CoM and joint of Robot Platform L3 = 0.1;                  % Distance between joint and fron twheel of Robot Platform h1 = 0.05;                 % Height above ground joint Robot Platform h2 = 0.5;                  % Length Pole m1 = 2;                    % Mass Robot Platform m2 = 6;                    % Mass Pole code =Define Robot Links with DH parameters= math \begin{array}{c|c|c|c|c|c|c|c|c} \text{Link} & \theta_i & d_i & a_i & \alpha_i & \sigma_i & \text{CoM} & \text{Mass} & \text{Inertia} \\ \hline 1 & 0 & h_1 & L_2 & \pi / 2 & R & \begin{bmatrix} -L_2 \\ -h_1 /2 \\ 0 \end{bmatrix} & m_1 & \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix} \\ \hline 2 & 0 & 0 & h_2 & 0 & R & \begin{bmatrix} -h_2 /2 \\ 0 \\ 0 \end{bmatrix} & m_2 & \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & I_{zz} \end{bmatrix} \\ \end{array} math

where math I_{zz} = \frac{1}{12} m_2 h_2^2 math

code format="matlab" % Link 1: Moving Robot Platform LK{1}.theta = 0;           % Rotation about z0 axis LK{1}.d = h1;              % Height of rotation joint between base and torso LK{1}.a = L2;              % Displacement of joint along x-axis LK{1}.alpha = pi/2;        % Joint axis (rotation around x axis) LK{1}.sigma = 'R';         % Revolute joint LK{1}.r = [-L2; -h1/2; 0]; % Center of mass LK{1}.m = m1;              % Mass LK{1}.I = diag([0, 0, 0]); % Inertia

% Link 2: Pole Izz = 1/12*m2*h2^2;        % Inertia around z axis LK{2}.theta = 0;          % Rotation about z0 axis LK{2}.d = 0;               % Height of rotation joint between base and torso LK{2}.a = h2;              % Displacement of joint along x-axis LK{2}.alpha = 0;           % Joint axis (rotation around x axis) LK{2}.sigma = 'R';         % Revolute joint LK{2}.r = [-h2/2; 0; 0];   % Center of mass LK{2}.m = m2;              % Mass LK{2}.I = diag([0, 0, Izz]);% Inertia code

=Create Robot Configuration= The ZMP algorithm needs a 'Conf' structure defining the robot. This structure has 3 properties, 'Base', 'Torso' and 'Arms'. The Base is needed to perform te mobile robot movements, i.e. driving. Base is a special structure which is initialized automatically and needs no user input. The Torso and Arms structures need to be configured by the user using the above 'LK' structure. In this case, the robot can be modeled with only a Torso consisting of two links (LK). Arms can be used to create a diverging chain of links, for instance a Torso with two arms attached to its shoulders.


 * Config file/element for complete mobile robot**
 * Conf.Torso
 * Conf.Arms{1}
 * Conf.Arms{2}
 * Conf.Arms{n}


 * Conf.Torso:**
 * Conf.Torso.LK{1}
 * Conf.Torso.LK{2}
 * Conf.Torso.LK{n}


 * Conf.Arms:**
 * Conf.Arms{1}.LK{1}
 * Conf.Arms{n}.LK{n}


 * Each Link (LK) has:**
 * LK.theta
 * LK.d
 * LK.a
 * LK.alpha
 * LK.sigma
 * LK.r
 * LK.m
 * LK.I

code format="matlab" Conf.Torso.LK = LK; code

=Motion Profile= Load coordinates from file. Coordinates were created with the 'advanced setpoints' tool found on the Mathworks site: @http://www.mathworks.nl/matlabcentral/fileexchange/16352-advanced-setpoints-for-motion-systems

code format="matlab" load('MotProf') t = MotProf.t;             % Time vector q = MotProf.q;             % Position vector qd = MotProf.qd;           % Velocity vector qdd = MotProf.qdd;         % Acceleration vector

% Plotten figure(1) subplot(3,1,1) plot(t,qdd) ylabel('acc [m/s2]') grid on subplot(3,1,2) plot(t,qd) ylabel('vel [m/s]') grid on subplot(3,1,3) plot(t,q) ylabel('pos [m]') xlabel('time [s]') grid on code

= = =Robot joint coordinates= The ZMP algorithm needs joint coordinates for every link of the Robot. The time derivatives (velocity and acceleration) are also needed. The general structure looks like this:
 * Coords.Base
 * Coords.Torso
 * Coords.Arms{1}
 * Coords.Arms{2}
 * Coords.Arms{n}


 * Coords.Base:** The robot Base has 6 DoF, so it needs 6x3 coordinates.
 * Coords.Base.q -> [Rx, Ry, Rz, Tx, Ty, Tz]
 * Coords.Base.qd -> [Rdx, Rdy, Rdz, Tdx, Tdy, Tdz]
 * Coords.Base.qdd -> [Rddx, Rddy, Rddz, Tddx, Tddy, Tddz]


 * Coords.Torso:** The Torso needs 3 coordinates per link (position, velocity and acceleration).
 * Coords.Torso.LK{1} -> [q1, qd1, qdd1]


 * Coords.Arms:** Per arm and per link there are 3 coordinates needed (position, velocity and acceleration).
 * Coords.Arms{1}.LK{1} -> [q11, qd11, qdd11]
 * Coords.Arms{1}.LK{2} -> [q12, qd12, qdd12]
 * Coords.Arms{2}.LK{1} -> [q21, qd21, qdd21]
 * Coords.Arms{2}.LK{2} -> [q22, qd22, qdd22]

=Base Coordinates= The mobile robot only moves in x-direction, so it only needs the Tx, Tdx and Tddx coordinates. code format="matlab" zer = zeros(size(t)); % empty coordinates vector Coords.Base.q = [zer, zer, zer, q, zer, zer]; Coords.Base.qd = [zer, zer, zer, qd, zer, zer]; Coords.Base.qdd = [zer, zer, zer, qdd, zer, zer]; code = = =Torso Coordinates= Link 1: Moving robot platform. The movement of this platform is done via the Robot Base coordinates above. This link is only a static transformation from the (moving) reference frame based on the floor to the rotational joint of the pole. Link 2: Rotating pole. In this case, the pole does not rotate but stays static at a 90 degree angle. code format="matlab" Coords.Torso.LK{1} = [zer, zer, zer];
 * Coords.Base.q -> [Rx, Ry, Rz, Tx, Ty, Tz]
 * Coords.Base.qd -> [Rdx, Rdy, Rdz, Tdx, Tdy, Tdz]
 * Coords.Base.qdd -> [Rddx, Rddy, Rddz, Tddx, Tddy, Tddz]

deg = pi/180; theta = 90*deg; q2 = ones(size(t))*theta; Coords.Torso.LK{2} = [q2, zer, zer]; code

=Time vector= The time vector is also needed when calculating the ZMP code format="matlab" Coords.t = t; code

=Put Robot Links in structure= Create a complete structure for the ZMP algorithm. All link information and coordinates go into this structure. The ZMP algorithm also uses this structure to store and exchange some intermediate results. This structure is automatically created with the function 'ZMP_create_struct'


 * ZMP_Robot Config:**
 * ZMP_Robot.g0
 * ZMP_Robot.theta
 * ZMP_Robot.d
 * ZMP_Robot.a
 * ZMP_Robot.alpha
 * ZMP_Robot.sigma
 * ZMP_Robot.r
 * ZMP_Robot.m
 * ZMP_Robot.I
 * ZMP_Robot.Type
 * ZMP_Robot.ArmID
 * ZMP_Robot.PrevLink
 * ZMP_Robot.NumberOfLinks


 * ZMP_Robot Coords:**
 * ZMP_Robot.q
 * ZMP_Robot.qd
 * ZMP_Robot.qdd


 * ZMP_Robot Results:**
 * ZMP_Robot.g
 * ZMP_Robot.T0_i
 * ZMP_Robot.r_ci
 * ZMP_Robot.W_i
 * ZMP_Robot.Wd_i
 * ZMP_Robot.rdd_ci
 * ZMP_Robot.rdd_i
 * ZMP_Robot.I0

The first entries are the same as before, link information: DH parameters, mass and inertia. One other entry is the standard gravity. The last entries are:
 * Robot Config Structure**
 * Type of a link ('Base', 'Torso', 'Arm')
 * ArmID: the arm that contains this link
 * LinkID: link number within link type
 * PrevLink: Previous link in kinematic chain
 * NumberOfLinks: Total number of links including base

Here the coordinates for all links are stored.
 * Robot Coords Structure**

Intermediate results from ZMP calculations are stored here. code format="matlab" g0 = [0; 0; -9.80665]; [ZMP_Robot] = ZMP_create_struct(Conf,Coords,g0) % Creates above structure code code format="matlab" ZMP_Robot =
 * Robot Results Structure**
 * g: transformed gravity (due to driving on an inclined plane)
 * T0_i: Homogeneous Transformation Matrix from Reference frame 0 to link frame i
 * r_ci: position vector from link frame i to CoM
 * W_i: rotational velocity for frame i
 * Wd_i: rotational acceleration for frame i
 * rdd_ci: linear acceleration of Center of Mass of link i
 * rdd_i: linear acceleration of frame i
 * I0: Inertia matrix with respect to reference frame 0

g0: [3x1 double] theta: {[0] [0]  [0]} d: {[0] [0.0500]  [0]} a: {[0] [0.0500]  [0.5000]} alpha: {[0] [1.5708]  [0]} sigma: {'B' 'R'  'R'} r: {[3x1 double] [3x1 double]  [3x1 double]} m: {[0] [2]  [6]} I: {[3x3 double] [3x3 double]  [3x3 double]} Type: {'Base' 'Torso'  'Torso'} ArmID: {[0] [0]  [0]} LinkID: {[1] [1]  [2]} PrevLink: {[0] [1]  [2]} NumberOfLinks: 3 t: [229x1 double] q: {[229x6 double] [229x1 double]  [229x1 double]} qd: {[229x6 double] [229x1 double]  [229x1 double]} qdd: {[229x6 double] [229x1 double]  [229x1 double]} g: [3x1 double] T0_i: {[4x4 double] [4x4 double]  [4x4 double]} r_ci: {[3x1 double] [3x1 double]  [3x1 double]} W_i: {[3x1 double] [3x1 double]  [3x1 double]} Wd_i: {[3x1 double] [3x1 double]  [3x1 double]} rdd_ci: {[3x1 double] [3x1 double]  [3x1 double]} rdd_i: {[3x1 double] [3x1 double]  [3x1 double]} I0: {[3x3 double] [3x3 double]  [3x3 double]} code

=ZMP Calculation= When the ZMP Robot structure is created, the computation of the ZMP can be done with the function 'ZMP_main'. The resulting 'r_zmp' array has a x-y-z position of the ZMP for every time instance. The IntRes result is a structure with some other interesting results. r_com is the location of the center of mass. Pd is the change in linear momentum, Hd is the change in angular momentum and g is the transformed gravity. code format="matlab" [r_zmp, IntRes] = ZMP_main(ZMP_Robot); code

=Plot results= code format="matlab" figure(2) plot(t,r_zmp(1,:)); grid on xlabel('time [s]') ylabel('x-location of ZMP') title('x location of ZMP')

figure(3) plot(t,IntRes.Hd(2,:)) grid on xlabel('time [s]') ylabel('change in angular momentum') title('Change in Angular Momentum') code



=Save Robot Structure for future use= The Robot Structure 'ZMP_Robot' can be saved to file to be used again later. It is named 'Tut1_Robot.mat'. code format="matlab" save('Tut1_Robot.mat','ZMP_Robot') code This file can be downloaded here:



=Tutorial 2:= continue to tutorial 2