def test_stabilizes_theta(self):
"""Verify that the hopper stabilizes both hopping and theta"""
from hopper_2d import Simulate2dHopper
x0 = np.zeros(10)
x0[1] = 2. # in air
x0[2] = -0.1 # start tilted a bit
x0[4] = 0.5 # feasible leg length
x0[5] = 0.1 # small lateral velocity
x0[7] = -0.1 # Base running away
T = 10
hopper, controller, state_log = \
Simulate2dHopper(x0 = x0,
duration = T,
desired_lateral_velocity = 0.0)
# Three seconds used as that's safely longer than the
# typical bouncing period of this system
# with the default spring / bouncing controller
index_of_last_three_seconds = \
np.argmax(state_log.sample_times() > T-3)
body_z_history = state_log.data()[1, index_of_last_three_seconds:]
# Look at theta history across all time -- good tracking means
# this should *never* deviate too wildly
body_theta_history = state_log.data()[2, :]
theta_lim = 0.5
theta_was_stable = \
np.all(body_theta_history > -theta_lim) and \
np.all(body_theta_history < theta_lim)
# Full leg extension is 1.5 off the ground
theta_max_stance_height = 1.5
z_indicates_a_bounce = \
np.any(body_z_history > theta_max_stance_height) and \
np.any(body_z_history <= theta_max_stance_height)
self.assertTrue(
theta_was_stable, "Theta was outside of [-%f, %f] during the "
"last three seconds of a %f second simulation from "
"x0 = %s, indicating your hopper didn't stabilize theta." %
(theta_lim, theta_lim, T, np.array_str(x0)))
self.assertTrue(
z_indicates_a_bounce,
"Bouncing appears to have stopped by the last three seconds "
"of a %f second simulation from x0 = %s, as indicated by "
"z being either always above, or always below, z=%f." %
(T, np.array_str(x0), theta_max_stance_height))
def test_continues_hopping(self):
"""Verify that the hopper keeps hopping for 10s"""
from hopper_2d import Simulate2dHopper
x0 = np.zeros(10)
x0[1] = 2. # in air
x0[4] = 0.5 # feasible leg length
x0[5] = 0.1 # initial speed
T = 10
hopper, controller, state_log = \
Simulate2dHopper(x0 = x0,
duration = T,
desired_lateral_velocity = 0.0)
# Three seconds used as that's safely longer than the
# typical bouncing period of this system
# with the default spring / bouncing controller
index_of_last_three_seconds = \
np.argmax(state_log.sample_times() > T-3)
body_z_history = state_log.data()[1, index_of_last_three_seconds:]
body_theta_history = state_log.data()[2, index_of_last_three_seconds]
# Full leg extension is 1.5 off the ground
theta_max_stance_height = 1.5
z_indicates_a_bounce = \
np.any(body_z_history > theta_max_stance_height) and \
np.any(body_z_history <= theta_max_stance_height)
self.assertTrue(
z_indicates_a_bounce,
"Bouncing appears to have stopped by the last three seconds "
"of a %f second simulation from x0 = %s, as indicated by "
"z being either always above, or always below, z=%f." %
(T, np.array_str(x0), theta_max_stance_height))
def test_lateral_velocity(self):
"""Verify that the hopper tracks a desired lateral velocity
while stabilizing theta and hopping"""
from hopper_2d import Simulate2dHopper
x0 = np.zeros(10)
x0[1] = 2. # in air
x0[4] = 0.5 # feasible leg length
x0[5] = 0.1 # initial speed
T = 10
desired_lateral_velocity = 0.5
hopper, controller, state_log = \
Simulate2dHopper(x0 = x0,
duration = T,
desired_lateral_velocity = desired_lateral_velocity)
# Three seconds used as that's safely longer than the
# typical bouncing period of this system
# with the default spring / bouncing controller
index_of_last_three_seconds = \
np.argmax(state_log.sample_times() > T-3)
body_z_history = state_log.data()[1, index_of_last_three_seconds:]
body_xd_history = state_log.data()[5, index_of_last_three_seconds:]
# Look at theta history across all time -- good tracking means
# this should *never* deviate too wildly
body_theta_history = state_log.data()[2, :]
theta_lim = 0.5
theta_was_stable = \
np.all(body_theta_history > -theta_lim) and \
np.all(body_theta_history < theta_lim)
# Full leg extension is 1.5 off the ground
theta_max_stance_height = 1.5
z_indicates_a_bounce = \
np.any(body_z_history > theta_max_stance_height) and \
np.any(body_z_history <= theta_max_stance_height)
# Really liberal window on desired velocity
min_desired_velocity = desired_lateral_velocity * 0.25
xd_indicates_velocity_tracking = \
np.all(body_xd_history > min_desired_velocity)
self.assertTrue(
theta_was_stable, "Theta was outside of [-%f, %f] during the "
"last three seconds of a %f second simulation from "
"x0 = %s, indicating your hopper didn't stabilize theta "
"with desired lateral velocity %f." %
(theta_lim, theta_lim, T, np.array_str(x0),
desired_lateral_velocity))
self.assertTrue(
z_indicates_a_bounce,
"Bouncing appears to have stopped by the last three seconds "
"of a %f second simulation from x0 = %s, as indicated by "
"z being either always above, or always below, z=%f, with "
"desired lateral velocity of %f." %
(T, np.array_str(x0), theta_max_stance_height,
desired_lateral_velocity))
self.assertTrue(
min_desired_velocity,
"Velocity was not always > %f during the last three seconds "
"of a %f second simulation from x0 = %s with desired lateral "
"velocity %f." % (min_desired_velocity, T, np.array_str(x0),
desired_lateral_velocity))