Robot Controllers Part 2: Raibert Hopper (with Dr. Christian Hubicki)

run1DHopperSim.m

% run1DHopperSim.m
% Simple 1D Hopper Simulation Script
% Christian Hubicki
% 2025-08-13 CE
% Simulation of a vertical hopping robot. As simple and readable as is
% reasonable

% Clear everything
clc; clear; close all;

% Set physical parameters
mass = 1; % kg
gravity = 9.8; % m/s^2
springRestLength = 1; % m
springStiffness = 100; % N/m

% Set simulation parameters
timeStep = 0.0005; % seconds
totalTime = 5; % seconds
numSteps = totalTime / timeStep;
time = linspace(0, totalTime, numSteps);

% Initial Conditions
initialPosition = springRestLength*1.1; % m
initialVelocity = 0; % m/s

% Initialize position and velocity arrays
position = zeros(numSteps, 1);
velocity = zeros(numSteps, 1);
position(1) = initialPosition;
velocity(1) = initialVelocity;

% Preallocate array for locomotion phase
locomotionPhase = zeros(numSteps, 1);

% Simulation loop
for iter = 1:numSteps-1
    % Calculate forces
    if(position(iter) > springRestLength) % In flight phase
        springForce = 0;
        locomotionPhase(iter) = 0; % Flight flagged as 0
    else % In stance phase
        springForce = -springStiffness * (position(iter) - springRestLength);
        locomotionPhase(iter) = 1; % Stance flagged as 1
    end
    weight = mass * gravity;
    netForce = springForce - weight;

    % Update acceleration, velocity, and position
    acceleration = netForce / mass;
    % Use Euler's method to update position and velocity
    velocity(iter + 1) = velocity(iter) + acceleration * timeStep;
    position(iter + 1) = position(iter) + velocity(iter) * timeStep;
end

% Parse position data into flight and stance phases
flightIndices = find(locomotionPhase == 0);
stanceIndices = find(locomotionPhase == 1);
flightPosition = nan(numSteps,1); % preallocate with NaNs
flightPosition(flightIndices) = position(flightIndices);
stancePosition = nan(numSteps,1); % preallocate with NaNs
stancePosition(stanceIndices) = position(stanceIndices);

% Plot the results
figure;
hold on
% Plot blue for flight data
plot(time, flightPosition, 'b-', 'LineWidth', 2)
% Plot red for stance data
plot(time, stancePosition, 'r-', 'LineWidth', 2)
set(gca,'ylim',[0 1.5])
set(gcf,'color',[1 1 1])
hold off

xlabel('Time (s)');
ylabel('Position (m)');
title('1D Hopper Height Over Time');
grid on;

run1DHoppingControllerSim.m

% run1DHoppingControllerSim.m
% Simple 1D Hopper Simulation Script
% Christian Hubicki
% 2025-08-13 CE
% Simulation of a vertical hopping robot. As simple and readable as is
% reasonable

% Clear everything
clc; clear; close all;

% Set physical parameters
mass = 1; % kg
gravity = 9.8; % m/s^2
nominalSpringRestLength = 1; % m
springStiffness = 100; % N/m
dampingCoefficient = 1; % Ns/m

% Set simulation parameters
timeStep = 0.0005; % seconds
totalTime = 10; % seconds
numSteps = totalTime / timeStep;
time = linspace(0, totalTime, numSteps);

% Define controller parameters
desiredApexHeight = 1.25; % Target height for the hopper
kp = 0.4; % Proportional gain for the controller
ki = 0.2;
kd = 0.2; % 0.2; % Derivative gain for the controller

% Initial Conditions
initialPosition = nominalSpringRestLength*1.1; % m
initialVelocity = 0; % m/s

% Initialize position and velocity arrays
position = zeros(numSteps, 1);
velocity = zeros(numSteps, 1);
position(1) = initialPosition;
velocity(1) = initialVelocity;

% Initialize internal controller variables
currentSpringRestLength = nominalSpringRestLength; % m
currentApexHeight = initialPosition;
currentBottomHeight = initialPosition; 
priorApexError = desiredApexHeight - initialPosition;
apexErrorIntegral = 0;

% Preallocate array for locomotion phase
locomotionPhase = zeros(numSteps, 1);
netForceArray = zeros(numSteps, 1);

% State machine flag
% 1 - Flight phase (pre-apex)
% 2 - Flight phase (post-apex)
% 3 - Stance phase (pre-bottom)
% 4 - Stance phase (post-bottom)
phaseFlag = 1;

% Simulation loop
for iter = 1:numSteps-1

    switch phaseFlag
        case 1
            locomotionPhase(iter) = 1; % Flight (pre-Apex) flagged as 1
            springForce = 0;
            dampingForce = 0;

            if(position(iter) > currentApexHeight) % Height is climbing, update apex
                currentApexHeight = position(iter);
            else
                phaseFlag = 2; % Transition to post-apex flight phase
                currentApexHeight
            end
        case 2
            locomotionPhase(iter) = 2; % Flight (post-Apex) flagged as 2
            springForce = 0;
            dampingForce = 0;
            
            if(position(iter) <= nominalSpringRestLength) 
                phaseFlag = 3; % Transition to stance phase (pre-bottom)
                dampingForce = -dampingCoefficient * velocity(iter);
            end
        case 3
            locomotionPhase(iter) = 3; % Stance (pre-Bottom) flagged as 3
            springForce = -springStiffness * (position(iter) - currentSpringRestLength);
            dampingForce = -dampingCoefficient * velocity(iter);

            if(position(iter) < currentBottomHeight) % Height is climbing, update apex
                currentBottomHeight = position(iter);
            else
                phaseFlag = 4; % Transition to post-bottom stance phase


                proportionalApexError = desiredApexHeight - currentApexHeight;
                derivativeApexError = -1*(proportionalApexError - priorApexError);
                priorApexError = proportionalApexError; % Update prior apex height

                apexErrorIntegral = apexErrorIntegral + proportionalApexError;

                currentSpringRestLength = nominalSpringRestLength + ...
                    kp * proportionalApexError + ...
                    ki * apexErrorIntegral + ...
                    kd * derivativeApexError;
            end
        case 4
            locomotionPhase(iter) = 4; % Stance (post-Bottom) flagged as 4
            springForce = -springStiffness * (position(iter) - currentSpringRestLength);
            dampingForce = -dampingCoefficient * velocity(iter);

            if(position(iter) > currentSpringRestLength) 
                phaseFlag = 1; % Transition to flight phase (pre-Apex)
                currentSpringRestLength = position(iter);

                % Reset apex and bottom to current position
                currentApexHeight = 0; %position(iter);
                currentBottomHeight = 10; %position(iter);
            end
    end

    % Calculate forces
    weight = mass * gravity;
    netForce = springForce + dampingForce - weight;

    % Store forces in the array for plotting
    netForceArray(iter) = netForce + weight;

    % Update acceleration, velocity, and position
    acceleration = netForce / mass;
    % Use Euler's method to update position and velocity
    velocity(iter + 1) = velocity(iter) + acceleration * timeStep;
    position(iter + 1) = position(iter) + velocity(iter) * timeStep;

end

% Parse position data into flight and stance phases
flightIndices = find(locomotionPhase <= 2);
stanceIndices = find(locomotionPhase >= 3);
flightPosition = nan(numSteps,1); % preallocate with NaNs
flightPosition(flightIndices) = position(flightIndices);
stancePosition = nan(numSteps,1); % preallocate with NaNs
stancePosition(stanceIndices) = position(stanceIndices);

% Plot the hopping height results
figure
hold on
% Plot target line
plot([0 totalTime],[desiredApexHeight desiredApexHeight],'k--')
% Plot blue for flight data
plot(time, flightPosition, 'b-', 'LineWidth', 2)
% Plot red for stance data
plot(time, stancePosition, 'r-', 'LineWidth', 2)

axis equal
set(gca,'xlim',[0 totalTime])
set(gca,'ylim',[0 2])
set(gcf,'color',[1 1 1])
hold off

xlabel('Time (s)');
ylabel('Position (m)');
title('1D Hopping Controller Height Over Time');
grid on;