Zero+Moment+Point+Tutorial+5+-+SimMechanics+and+Flexibilities

=Define Robot= Load robot structure from file:

SimMechanics model:

Tutorial as Matlab script:

code format="matlab" %% Define Robot load Tut5_Robot

% 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 L2 = 0.8; % Length Torso (height) r2 = 0.2; % m3 = 20; % Arm1 Link 1 A1L1 = 0.5; % Length of arm A1r1 = 0.04; % Radius arm A1m = 7; % Weight arm link % Arm1 Link 2 A1L2 = 0.5; % Length of arm A1r2 = 0.04; % Radius arm % Arm Link 3 % Load: Point mass A1m3 = 2; % Load of 2 kg

% First time vector ts = 0; tf = 5; nt = 51; t = linspace(ts,tf,nt); code = = =Create SimMechanics Robot structure= code format="matlab" %% Create SimMechanics Robot structure [SM_ZMP_Base, SM_ZMP_Torso, SM_ZMP_Arms] = CreateSMstruct(ZMP_Robot) code

=SimMechanics Flexibility= To add flexibility to the SimMechanics model, the standard joints can be used. These joints can be given a desired spring stiffness and damping.

The Flexibility of the robot base (stiffness of suspension) is shown below:

Flexibilities in the robot arms are shown below:

=Calculate Spring Stiffness for Joint Stiffness Simulation= code format="matlab" %% Joint stiffness % DESIRED VALUES % Desired natural frequency: f0_1 = 3; % Herz, in high config of entire robot base f0_2 = 10; % Torso Rotational Hz f0_3 = 10; % Torso Translational Hz % Arms have same natural frequency as Torso Rotational % % Damping ratios zeta_1 = 0.1; % Not well damped robot base (stiff suspension) zeta = 0.7; % TORSO Rotational, Translational and Arms

% Masses and heights PM_H1 = H1/2; % Height of first point mass PM_M1 = m1; % mass of point mass1 (Base) PM_H2l = PM_H1 + L2/2; % Height of second point mass (Torso in lowered state) PM_H2h = PM_H2l + L2; % Torso in elevated state PM_M2 = m3; % mass of point mass2 (Torso) PM_H3l = PM_H2l + L2/2; PM_H3h = PM_H2h + L2/2; % Torso in elevated state PM_M3 = 2*A1m; % mass of point mass3 (Arms)

% Computation of inertia due to point masses IR_low = PM_M1*PM_H1^2 + PM_M2*PM_H2l^2 + 2*PM_M3*PM_H3l^2; % at lowerd torso IR_high = PM_M1*PM_H1^2 + PM_M2*PM_H2h^2 + 2*PM_M3*PM_H3h^2; % at elevated torso

% BASE % Stiffness of ball joint (rotational spring) kb_1 = 4*pi^2*f0_1^2*IR_high; % Rotational spring stiffness db_1 = 2*zeta_1*sqrt(IR_high*kb_1); % Rotational damper

% TORSO Rotational IT = PM_M3*(A1L1/2)^2 + PM_M3*A1L1^2; kb_2 = 4*pi^2*f0_2^2*IT; % Rotational sring stiffness db_2 = 2*zeta*sqrt(IT*kb_2);

% TORSO Translational PM_MT = PM_M2 + 2*PM_M3; % Mass of Torso and Arms kb_3 = 4*pi^2*f0_3^2*PM_MT; % translational spring db_3 = 2*zeta*sqrt(PM_MT*kb_3); % translational damper

% Upper Arms kb_4 = kb_2; db_4 = db_2;

% Lower Arms IAl = PM_M3*(A1L1/2)^2; kb_5 = 4*pi^2*f0_3^2*IAl; db_5 = 2*zeta*sqrt(IAl*kb_5); code

=Computations= download SimMechanics robot model:

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

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

=Plot Results= code format="matlab" %% 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),':',IntRes.r_com(1,:),IntRes.r_com(2,:),'--') 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') code