Zero+Moment+Point+Tutorial+4+-+Mobile+Service+Robot

This tutorial is also available as Matlab script:

It is important to note that in this script and this tutorial the Robotics Toolbox from Peter Corke is used. This toolbox can be downloaded from: @http://www.petercorke.com/RVC/top/toolboxes/ The SerialLink functionality from that toolbox is used to create the torso and arm structures. These can also be created with the definitions shown in tutorial 1. Also, the motion profile is created with the LSPB function from that toolbox.

In this tutorial the following SimMechanics model is used:

The model is adapted to incorporate a 3-link torso and two 3-link arms. Also some visuals are added to the model. = = =Define Robot= The robot we are going to model is shown in the figure below:

Torso
First link of the torso will be the moving base of the robot with a high mass. Second link and third link are the torso of the robot, it can translate along the z-axis and rotate about this z axis. code format="matlab" %% Torso % Link 1: Torso (not actuated) % Moving base L1 = 0.60; % Length Base frame W1 = 0.40; % Width Base frame H1 = 0.25; % Height Base frame m1 = 40; % mass base frame LT(1) = Link; LT(1).d = H1; LT(1).a = 0; LT(1).m = m1; LT(1).r = [0; 0; -H1/2]; LT(1).sigma = 0; % Revolute, but not actuated. % Inertia of Base (box with longest length in x-direction) mI = LT(1).m; %mI = 500; II1 = 1/12*mI*[(W1^2+H1^2)  0               0 0             (L1^2+H1^2)     0 0             0               (L1^2+W1^2)]; LT(1).I = II1;

% Link 2: Torso (actuated) % Link 2 is the torso itself, the revolute part % This link has no mass or inertia, it is only a link to establish a % revolute joint. L2 = 0.8; % Length Torso (height) r2 = 0.2; % LT(2) = Link; LT(2).d = L2; % Starting height of torso LT(2).m = 0; LT(2).sigma = 0; % Revolute

% Link 3: Torso (actuated) % Link 3 is the torso itself, the prismatic part % Torso is cylinder with length L and radius r m3 = 20; LT(3) = Link; LT(3).d = 0; % actuated height LT(3).alpha = pi/2; % Rotated 90 deg around x-axis, for arm joints LT(3).sigma = 1; % Prismatic LT(3).qlim = [0, L2]; % Torso can stretch a maximum of L2 LT(3).m = m3; LT(3).r = [0; -L2/2; 0]; % Inertia (cylinder) mI = LT(3).m; II3 = 1/12*mI*[(3*r2^2 + L2^2)  0          0 0                  6*r2^2     0 0                 0           (3*r2^2 + L2^2)]; LT(3).I = II3; % Inertia Torso

% Create a structure for the Torso with Peter Corkes SerialLink function TorsoPC = SerialLink(LT);      % Peter Corke's SerialLink object Torso = ZMP_RVC2ZMP(TorsoPC);  % Torso in ZMP format

code

Arm 1
code format="matlab" %% Arm 1

% Arm1 Link 1 A1L1 = 0.5; % Length of arm A1r1 = 0.04; % Radius arm A1m = 7; % Weight arm link

A1(1) = Link; A1(1).m = A1m; A1(1).d = r2; % Arm offset (shoulder width) A1(1).a = A1L1; % Arm length A1(1).sigma = 0; % Revolute A1(1).r = [-A1L1/2; 0; 0]; mI = A1(1).m; II4 = 1/12*mI*[6*A1r1^2 0                      0 0        (3*A1r1^2 + A1L1^2)    0 0        0                      (3*A1r1^2 + A1L1^2)]; A1(1).I = II4; % Inertia Arm

% Arm1 Link 2 A1L2 = 0.5; % Length of arm A1r2 = 0.04; % Radius arm A1m = 7; % Weight arm link

A1(2) = Link; A1(2).m = A1m; A1(2).d = 0; % Arm offset (shoulder width) A1(2).a = A1L2; % Arm length A1(2).sigma = 0; % Revolute A1(2).r = [-A1L2/2; 0; 0]; mI = A1(2).m; II5 = 1/12*mI*[6*A1r2^2 0                      0 0        (3*A1r2^2 + A1L2^2)    0 0        0                      (3*A1r2^2 + A1L2^2)]; A1(2).I = II5; % Inertia Arm

% Arm Link 3 % Load: Point mass A1m3 = 2; % Load of 2 kg A1(3) = Link; A1(3).sigma = 0; % Not actuated Revolute A1(3).m = A1m3;

% Create a structure for the first arm with Peter Corkes SerialLink function Arm1PC = SerialLink(A1);       % Peter Corke's SerialLink object Arm1 = ZMP_RVC2ZMP(Arm1PC);    % ZMP Format

code

Arm 2
code format="matlab" %% Arm 2 % Arm2 Link 1 % Arm 2 is a copy of arm 1, but shoulder shifted A2(1) = A1(1); A2(1).d = -r2;

% Arm2 Link 2 A2(2) = A1(2);

% Arm2 Link 3 A2(3) = A1(3);

% Create a structure for the second arm with Peter Corkes SerialLink function Arm2PC = SerialLink(A2);       % Peter Corke's SerialLink object Arm2 = ZMP_RVC2ZMP(Arm2PC);    % ZMP Format code

=Create Robot Configuration= Put the created robot structures in the right Conf structure: code format="matlab" %% Create Robot Configuration Conf.Torso.LK = Torso; Conf.Arms{1}.LK = Arm1; Conf.Arms{2}.LK = Arm2;

code =Create Coordinates= First, create a time vector. code format="matlab" % First time vector ts = 0; tf = 5; nt = 51; t = linspace(ts,tf,nt); Coords.t = t; code

Base movement
code format="matlab" % Base rides from 0 to 4 meter in 4 seconds x0 = 0; xf = 4;

[qx1, qdx1, qddx1] = lspb(x0,xf,t); zer = zeros(size(t,2),1);

Coords.Base.q = [zer, zer, zer, qx1, zer, zer]; Coords.Base.qd = [zer, zer, zer, qdx1, zer, zer]; Coords.Base.qdd = [zer, zer, zer, qddx1, zer, zer]; code

Torso Coordinates
code format="matlab" %% Coords.Torso: % Link 1: not actuated Coords.Torso.LK{1} = [zer, zer, zer];

% Link 2 % Torso rotates from -90 deg to 0 deg th0 = -pi/2; thf = 0;

[q2, qd2, qdd2] = lspb(th0,thf,t); Coords.Torso.LK{2} = [q2, qd2, qdd2];

% Link 3 % Torso moves up (stretches) from 0 to L2 d0 = 0; df = L2;

[q3, qd3, qdd3] = lspb(d0,df,t); Coords.Torso.LK{3} = [q3, qd3, qdd3]; code

Arm Coordinates
code format="matlab" %% Coords.Arms: % Link 1 % Upper arm moves from -90 to 0 th0 = -pi/2; thf = 0;

[q4, qd4, qdd4] = lspb(th0,thf,t); Coords.Arms{1}.LK{1} = [q4, qd4, qdd4];

% Link 2 % Under arm moves from 90 to 0 th0 = pi/2; thf = 0;

[q5, qd5, qdd5] = lspb(th0,thf,t); Coords.Arms{1}.LK{2} = [q5, qd5, qdd5];

% Link 3 (load, not moving) Coords.Arms{1}.LK{3} = [zer, zer, zer];

%% Arm 2 % Arm 2 is a copy of arm 1 Coords.Arms{2} = Coords.Arms{1}; code

=Put Robot Links in structure= code format="matlab" g0 = [0; 0; -9.80665]; [ZMP_Robot] = ZMP_create_struct(Conf,Coords,g0); % Creates robot structure code

=Create SimMechanics Robot structure= The used SimMechanics model can be downloaded:

code format="matlab" %% Create SimMechanics Robot structure [SM_ZMP_Base, SM_ZMP_Torso, SM_ZMP_Arms] = CreateSMstruct(ZMP_Robot)

code

=Run computations= code format="matlab" %% run zmp calculation [r_zmp, IntRes] = ZMP_main(ZMP_Robot)

%% Simulate SimMechanics model sim('Standard_Robot') code

=Plot results= First plot the used motion profile code format="matlab" %% Motion profile figure(1) plot(t,qx1,'-',t,qdx1,':',t,qddx1,'--') legend('q','qd','qdd','Location','NorthWest') grid on title('motion profile') xlabel('time [s]') ylabel('joint coordinate') code



Plot the resulting Zero Moment Point and show the velocity phase in the plot: code format="matlab" %% Find velocity phase in motion profile idx1 = find(qdx1==max(qdx1),1,'first'); idx2 = find(qdx1==max(qdx1),1,'last');

%% XY Zero Moment Point figure(2) plot(r_zmp(1,:),r_zmp(2,:),p_zmp_simu.data(2:2:end,1),p_zmp_simu.data(2:2:end,2),'o',IntRes.r_com(1,:),IntRes.r_com(2,:),'--') hold on legend('r_{P}: ZMP Algorithm','r_{P}: SimMechanics','r_{c}: COM','Location','NorthWest') grid on axis equal xlabel('x-position of ZMP/COM') ylabel('y-position of ZMP/COM') plot(r_zmp(1,idx1),r_zmp(2,idx1),'k.') plot(IntRes.r_com(1,idx1),IntRes.r_com(2,idx1),'k.') plot(r_zmp(1,idx2),r_zmp(2,idx2),'kx') plot(IntRes.r_com(1,idx2),IntRes.r_com(2,idx2),'kx') hold off code



link to tutorial 5