def test_rigidbody_defaults():
mass = 1.5
moment_of_inertia = np.diag([13.2e-3, 12.8e-3, 24.6e-3])
system = System()
rb_6dof = RigidBody6DOFFlatEarth(owner=system,
mass=mass,
moment_of_inertia=moment_of_inertia)
dcm_to_euler = DirectCosineToEuler(system)
thrust = constant(value=np.r_[0, 0, 0])
moment = constant(value=moment_of_inertia @ np.r_[0, 0, math.pi / 30**2])
thrust.connect(rb_6dof.forces_body)
moment.connect(rb_6dof.moments_body)
rb_6dof.dcm.connect(dcm_to_euler.dcm)
rtol = 1e-12
atol = 1e-12
sim = Simulator(system, start_time=0, rtol=rtol, atol=atol)
*_, last_item = sim.run_until(time_boundary=30.0, include_last=True)
npt.assert_allclose(dcm_to_euler.yaw(last_item),
math.pi / 2,
rtol=rtol,
atol=atol)
npt.assert_allclose(dcm_to_euler.pitch(last_item), 0, rtol=rtol, atol=atol)
npt.assert_allclose(dcm_to_euler.roll(last_item), 0, rtol=rtol, atol=atol)
npt.assert_allclose(rb_6dof.position_earth(last_item), [0, 0, 0],
rtol=rtol,
atol=atol)
def test_deprecated_options():
"""Test if deprecated options lead to warnings"""
system = System()
with pytest.warns(DeprecationWarning):
Simulator(system, start_time=0, event_xtol=1e-12)
with pytest.warns(DeprecationWarning):
Simulator(system, start_time=0, event_maxiter=1000)
def test_lti_simulation_failure(lti_system_with_reference):
sys, ref_function, sim_time, outputs = lti_system_with_reference
del ref_function, outputs # unused
simulator = Simulator(sys, start_time=0, solver_method=MockupIntegrator)
with pytest.raises(SimulationError):
for _ in simulator.run_until(sim_time):
pass
def test_clock_handling():
"""Test the handling of clocks in the simulator."""
time_constant = 1.0
initial_value = 2.0
system = System()
input_signal = constant(value=0.0)
lag = LTISystem(
parent=system,
system_matrix=-1 / time_constant,
input_matrix=1,
output_matrix=1,
feed_through_matrix=0,
initial_condition=[initial_value],
)
lag.input.connect(input_signal)
clock1 = Clock(system, period=0.2)
hold1 = zero_order_hold(
system,
input_port=lag.output,
event_port=clock1,
initial_condition=initial_value,
)
clock2 = Clock(system, period=0.25, end_time=2.0)
hold2 = zero_order_hold(
system,
input_port=lag.output,
event_port=clock2,
initial_condition=initial_value,
)
# Clock 3 will not fire within the simulation time frame
clock3 = Clock(system, period=0.25, start_time=-6.0, end_time=-1.0)
hold3 = zero_order_hold(
system,
input_port=lag.output,
event_port=clock3,
initial_condition=initial_value,
)
simulator = Simulator(system, start_time=0.0)
result = SimulationResult(system, simulator.run_until(time_boundary=5.0))
time_floor1 = np.floor(result.time / clock1.period) * clock1.period
time_floor2 = np.minimum(
clock2.end_time,
(np.floor(result.time / clock2.period) * clock2.period))
reference1 = initial_value * np.exp(-time_floor1 / time_constant)
reference2 = initial_value * np.exp(-time_floor2 / time_constant)
npt.assert_almost_equal(hold1(result), reference1)
npt.assert_almost_equal(hold2(result), reference2)
npt.assert_almost_equal(hold3(result), initial_value)
def test_integrator():
"""Test an integrator"""
system = System()
int_input = constant(1.0)
int_output = integrator(system, int_input)
simulator = Simulator(system, start_time=0.0)
result = SimulationResult(system, simulator.run_until(time_boundary=10.0))
npt.assert_almost_equal(int_output(result).ravel(), result.time)
def test_lti_simulation(lti_system_with_reference):
sys, ref_system, ref_time, _ = lti_system_with_reference
rtol = 1e-12
atol = 1e-12
simulator = Simulator(sys, start_time=0, rtol=rtol, atol=atol)
result = SimulationResult(sys)
result.collect_from(
simulator.run_until(time_boundary=ref_time / 2, include_last=False))
result.collect_from(
simulator.run_until(time_boundary=ref_time, include_last=False))
# Check that states are properly mapped in the result
for state in sys.states:
from_result = result.state[state.state_slice]
from_result = from_result.reshape(state.shape + (-1, ))
npt.assert_equal(state(result), from_result)
# Check that inputs are properly mapped in the result
for input_signal in sys.inputs:
from_result = result.inputs[input_signal.input_slice]
from_result = from_result.reshape(input_signal.shape + (-1, ))
npt.assert_equal(input_signal(result), from_result)
# Determine the system response and state values of the reference system
ref_time, ref_output, ref_state = scipy.signal.lsim2(
ref_system,
X0=sys.initial_condition,
T=result.time,
U=None,
rtol=rtol,
atol=atol,
)
del ref_output
# Determine the state values at the simulated times
# as per the reference value
npt.assert_allclose(ref_time, result.time, rtol=rtol, atol=atol)
# FIXME: These factors are somewhat arbitrary
npt.assert_allclose(result.state,
ref_state.T,
rtol=rtol * 1e2,
atol=atol * 1e2)
# Check that SimulationResult properly implements the sequence interface
assert len(result) == len(result.time)
for idx, item in enumerate(result):
npt.assert_equal(item.time, result.time[idx])
npt.assert_equal(item.state, result.state[:, idx])
npt.assert_equal(item.inputs, result.inputs[:, idx])
def test_saturation():
system = System()
Clock(system, period=0.01)
@signal_function
def _sine_source(system_state):
return np.sin(2 * np.pi * system_state.time)
saturated_out = saturation(_sine_source, lower_limit=-0.5, upper_limit=0.6)
simulator = Simulator(system, start_time=0.0)
result = SimulationResult(system, simulator.run_until(time_boundary=1.0))
sine_data = _sine_source(result)
saturated_data = saturated_out(result)
saturated_exp = np.minimum(np.maximum(sine_data, -0.5), 0.6)
npt.assert_almost_equal(saturated_data, saturated_exp)
def test_rigidbody_movement():
mass = 1.5
omega = 2 * math.pi / 120
r = 10.0
vx = r * omega
moment_of_inertia = np.diag([13.2e-3, 12.8e-3, 24.6e-3])
system = System()
rb_6dof = RigidBody6DOFFlatEarth(
owner=system,
mass=mass,
initial_velocity_earth=[vx, 0, 0],
initial_angular_rates_earth=[0, 0, omega],
initial_position_earth=[0, 0, 0],
initial_transformation=np.eye(3, 3),
moment_of_inertia=moment_of_inertia,
)
dcm_to_euler = DirectCosineToEuler(system)
thrust = constant(value=np.r_[0, mass * omega * vx, 0])
moment = constant(value=np.r_[0, 0, 0])
thrust.connect(rb_6dof.forces_body)
moment.connect(rb_6dof.moments_body)
rb_6dof.dcm.connect(dcm_to_euler.dcm)
rtol = 1e-12
atol = 1e-12
sim = Simulator(system, start_time=0, rtol=rtol, atol=atol)
*_, last_item = sim.run_until(time_boundary=30.0, include_last=True)
npt.assert_allclose(dcm_to_euler.yaw(last_item),
math.pi / 2,
rtol=rtol,
atol=atol)
npt.assert_allclose(dcm_to_euler.pitch(last_item), 0, rtol=rtol, atol=atol)
npt.assert_allclose(dcm_to_euler.roll(last_item), 0, rtol=rtol, atol=atol)
npt.assert_allclose(rb_6dof.position_earth(last_item), [r, r, 0],
rtol=rtol,
atol=atol)
npt.assert_allclose(rb_6dof.velocity_body(last_item), [vx, 0, 0],
rtol=rtol,
atol=atol)
npt.assert_allclose(rb_6dof.omega_body(last_item), [0, 0, omega],
rtol=rtol,
atol=atol)
def test_zero_crossing_event_detection():
"""Test the detection of zero-crossing events."""
x0 = 0
y0 = 10
vx0 = 1
vy0 = 0
g = 9.81
gamma = 0.7
system = System()
bouncing_ball = BouncingBall(parent=system, gravity=-g, gamma=gamma)
initial_condition = np.empty(system.num_states)
initial_condition[bouncing_ball.velocity.state_slice] = [vx0, vy0]
initial_condition[bouncing_ball.position.state_slice] = [x0, y0]
simulator = Simulator(
system,
start_time=0,
initial_condition=initial_condition,
max_step=0.05,
event_detector_options={"max_subdiv": 10},
)
result = SimulationResult(system, simulator.run_until(time_boundary=8.0))
# Determine the time of the first impact
t_impact = (2 * vy0 + math.sqrt(4 * vy0**2 + 8 * g * y0)) / (2 * g)
# Check that the y-component of velocity changes sign around time of impact
t_tolerance = 0.1
idx_start = bisect.bisect_left(result.time, t_impact - t_tolerance)
idx_end = bisect.bisect_left(result.time, t_impact + t_tolerance)
assert result.time[idx_start] < t_impact
assert result.time[idx_end] >= t_impact
vy = bouncing_ball.velocity(result)[1][idx_start:idx_end]
sign_change = np.sign(vy[:-1]) != np.sign(vy[1:])
assert any(sign_change)
# Check detection of excessive event error
with pytest.raises(ExcessiveEventError):
for _ in simulator.run_until(time_boundary=10.0):
pass
def test_system_state_updater_dictionary_access():
"""Test the deprecated dictionary access for simulation results"""
system = System()
counter = State(system, shape=(2, 2))
clock = Clock(system, 1.0)
def _update_counter(system_state):
old_val = counter(system_state).copy()
system_state[counter] += 1
system_state[counter, 0] += 1
system_state[counter, 0, 0] += 1
new_val = counter(system_state)
npt.assert_almost_equal(old_val + [[3, 2], [1, 1]], new_val)
clock.register_listener(_update_counter)
simulator = Simulator(system, start_time=0)
for _ in simulator.run_until(time_boundary=10):
pass
def test_excessive_events_second_level():
"""Test the detection of excessive events when it is introduced by
toggling the same event over and over."""
system = System()
state = State(system,
derivative_function=(lambda data: -1),
initial_condition=5)
event = ZeroCrossEventSource(system, event_function=state)
def event_handler(data):
"""Event handler for the zero-crossing event"""
state.set_value(data, -state(data))
event.register_listener(event_handler)
simulator = Simulator(system, start_time=0)
with pytest.raises(ExcessiveEventError):
for _ in simulator.run_until(6):
pass
def test_discrete_only():
"""Test a system with only discrete-time states."""
system = System()
clock = Clock(system, period=1.0)
counter = State(system)
def increase_counter(data):
"""Increase the counter whenever a clock event occurs"""
counter.set_value(data, counter(data) + 1)
clock.register_listener(increase_counter)
simulator = Simulator(system, start_time=0.0)
result = SimulationResult(system)
result.collect_from(simulator.run_until(5.5, include_last=False))
result.collect_from(simulator.run_until(10))
npt.assert_almost_equal(result.time,
[0, 1, 2, 3, 4, 5, 5.5, 6, 7, 8, 9, 10])
npt.assert_almost_equal(counter(result),
[1, 2, 3, 4, 5, 6, 6, 7, 8, 9, 10, 11])
dcm_to_euler = DirectCosineToEuler(system)
# We provide some thrust as centripetal force
thrust = constant(value=np.r_[0, MASS * OMEGA * VELOCITY_X, 0])
# No torques
torque = constant(value=np.r_[0, 0, 0])
# Connect thrust and torque to the rigid-body block
thrust.connect(rb_6dof.forces_body)
torque.connect(rb_6dof.moments_body)
# Connect the Direct Cosine Matrix output to the Euler conversion block
rb_6dof.dcm.connect(dcm_to_euler.dcm)
# Simulate the system for 2 minutes
sim = Simulator(system, start_time=0, max_step=0.1)
result = SimulationResult(system, sim.run_until(time_boundary=120.0))
# Plot the Euler angles
fig_euler, (ax_yaw, ax_pitch, ax_roll) = plt.subplots(nrows=3, sharex="row")
ax_yaw.plot(result.time, np.rad2deg(dcm_to_euler.yaw(result)))
ax_pitch.plot(result.time, np.rad2deg(dcm_to_euler.pitch(result)))
ax_roll.plot(result.time, np.rad2deg(dcm_to_euler.roll(result)))
ax_yaw.set_title("Yaw")
ax_pitch.set_title("Pitch")
ax_roll.set_title("Roll")
# Plot the trajectory in top view
fig_top_view, ax = plt.subplots()
position = rb_6dof.position_earth(result)
ax.plot(position[0], position[1])
# Create the states
velocity = State(
owner=system,
shape=2,
derivative_function=velocity_dt,
initial_condition=V_0,
)
position = State(owner=system,
shape=2,
derivative_function=velocity,
initial_condition=X_0)
# Run a simulation
simulator = Simulator(system, start_time=0.0, max_step=24 * 60 * 60)
result = SimulationResult(system,
simulator.run_until(time_boundary=ROTATION_TIME))
# Plot the result
trajectory = position(result)
plt.plot(trajectory[0], trajectory[1])
plt.gca().set_aspect("equal", "box")
plt.title("Planet Orbit")
plt.savefig("03_planet_orbit_trajectory.png")
plt.figure()
plt.plot(result.time, linalg.norm(trajectory, axis=0))
plt.xlabel("Time (s)")
plt.ylabel("Distance from the Sun (m)")
plt.title("Distance Earth-Sun over Time")
# Set up the state for keeping the sampled value.
sample_state = State(system)
# Create a clock for sampling at 100Hz
sample_clock = Clock(system, period=0.01)
# Define the function for updating the state
def update_sample(system_state):
"""Update the state of the sampler"""
sample_state.set_value(system_state, engine.thrust(system_state))
# Register it as event handler on the clock
sample_clock.register_listener(update_sample)
# Create the simulator and run it
simulator = Simulator(system, start_time=0.0)
result = SimulationResult(system, simulator.run_until(time_boundary=0.5))
# Plot the result
plt.plot(result.time, engine.thrust(result), "r", label="Continuous-Time")
plt.step(result.time, sample_state(result), "g", label="Sampled", where="post")
plt.title("Engine with DC-Motor and Static Propeller")
plt.legend()
plt.xlabel("Time")
plt.ylabel("Thrust")
plt.savefig("06_dc_engine_sampling.png")
plt.show()
# The system states
def velocity_dt(_system_state):
"""Calculate the derivative of the vertical speed"""
return -G
velocity = State(system,
derivative_function=velocity_dt,
initial_condition=INITIAL_VELOCITY)
height = integrator(system,
input_signal=velocity,
initial_condition=INITIAL_HEIGHT)
# Run a simulation without event handler
simulator = Simulator(system=system, start_time=0, max_step=0.1)
result = SimulationResult(system=system)
result.collect_from(simulator.run_until(time_boundary=2))
# Plot the simulation result
plt.plot(result.time, height(result))
plt.xlabel("Time (s)")
plt.ylabel("Height (m)")
plt.title("Falling Ball")
plt.grid(axis="y")
plt.savefig("04_bouncing_ball_no_event.png")
# Define a zero-crossing event for the height
bounce_event = ZeroCrossEventSource(system,
event_function=height,
direction=-1)