def test_without_goal():
"""Test the steady-state finding algorithm without objective function
and with no steady states"""
system = System()
config = SteadyStateConfiguration(system)
with pytest.raises(ValueError):
find_steady_state(config)
def test_invalid_objective():
"""Test the steady-state finding algorithm with an invalid objective"""
system = System()
config = SteadyStateConfiguration(system)
config.objective = True
with pytest.raises(ValueError):
find_steady_state(config)
def test_aerodyn_blocks(thrust_coefficient, power_coefficient):
system = System()
propeller = Propeller(
system,
thrust_coefficient=thrust_coefficient,
power_coefficient=power_coefficient,
diameter=8 * 25.4e-3,
)
dcmotor = DCMotor(
system,
motor_constant=789.0e-6,
resistance=43.3e-3,
inductance=1.9e-3,
moment_of_inertia=5.284e-6,
initial_omega=1,
)
thruster = Thruster(system,
vector=np.c_[0, 0, -1],
arm=np.c_[1, 1, 0],
direction=1)
density = constant(value=1.29)
gravity = constant(value=[0, 0, 1.5 / 4 * 9.81])
voltage = InputSignal(system)
voltage.connect(dcmotor.voltage)
density.connect(propeller.density)
dcmotor.speed_rps.connect(propeller.speed_rps)
propeller.torque.connect(dcmotor.external_torque)
propeller.thrust.connect(thruster.scalar_thrust)
dcmotor.torque.connect(thruster.scalar_torque)
force_sum = sum_signal((thruster.thrust_vector, gravity))
# Determine the steady state
steady_state_config = SteadyStateConfiguration(system)
# Enforce the sum of forces to be zero along the z-axis
steady_state_config.ports[force_sum].lower_bounds[2] = 0
steady_state_config.ports[force_sum].upper_bounds[2] = 0
# Ensure that the input voltage is non-negative
steady_state_config.inputs[voltage].lower_bounds = 0
sol = find_steady_state(steady_state_config)
assert sol.success
npt.assert_almost_equal(sol.state, [856.5771575, 67.0169392])
npt.assert_almost_equal(sol.inputs, [3.5776728])
npt.assert_almost_equal(propeller.power(sol.system_state), 45.2926865)
npt.assert_almost_equal(
np.ravel(thruster.torque_vector(sol.system_state)),
[-3.67875, 3.67875, -0.0528764],
)
def test_steady_state(config):
"""Test the find_steady_state function"""
# Adjust solver options
config.solver_options["gtol"] = 1e-10
sol = find_steady_state(config)
assert sol.success
# Structural checks
assert sol.state.size == config.system.num_states
assert sol.inputs.size == config.system.num_inputs
# Check state bounds
assert (-config.solver_options["gtol"] <=
(sol.state - config.state_bounds[:, 0])).all()
assert (-config.solver_options["gtol"] <=
(config.state_bounds[:, 1] - sol.state)).all()
# Check input bounds
assert (-config.solver_options["gtol"] <=
(sol.inputs - config.input_bounds[:, 0])).all()
assert (-config.solver_options["gtol"] <=
(config.input_bounds[:, 1] - sol.inputs)).all()
# Check port constraints
for signal_constraint in config.ports.values():
value = signal_constraint.port(sol.system_state)
assert (-config.solver_options["gtol"] <=
(value - signal_constraint.lb)).all()
assert (-config.solver_options["gtol"] <=
(signal_constraint.ub - value)).all()
# Check for steady states
num_steady_states = np.count_nonzero(config.steady_states)
diff_tol = np.sqrt(config.solver_options["gtol"] * num_steady_states)
steady_state_derivatives = config.system.state_derivative(
sol.system_state)[config.steady_states]
npt.assert_allclose(steady_state_derivatives.ravel(), 0, atol=diff_tol)
return (A1 * inflow_velocity(data) -
A2 * np.sqrt(2 * G * height_state(data))) / At
height_state = State(system, derivative_function=height_derivative)
# Configure for steady-state determination
steady_state_config = SteadyStateConfiguration(system)
# Enforce the height to equal the target height
steady_state_config.states[height_state].lower_bounds = TARGET_HEIGHT
steady_state_config.states[height_state].upper_bounds = TARGET_HEIGHT
# Enforce the inflow to be non-negative
steady_state_config.inputs[inflow_velocity].lower_bounds = 0
# Find the steady state
result = find_steady_state(steady_state_config)
print("Target height: %f" % TARGET_HEIGHT)
print("Steady state height: %f" % height_state(result.system_state))
print("Steady state inflow: %f" % inflow_velocity(result.system_state))
print("Steady state height derivative: %f" %
height_derivative(result.system_state))
print("Theoretical steady state inflow: %f" %
(np.sqrt(2 * G * TARGET_HEIGHT) * A2 / A1))
# Set up the configuration for finding the system jacobian
jacobian_config = LinearizationConfiguration(system,
state=result.state,
inputs=result.inputs)
# We want to have the height as output
output_1 = OutputDescriptor(jacobian_config, height_state)
def test_lti_linearization(param, interpolation_order):
system, lti, _, outputs = param
# Find the steady state of the system
steady_state_config = SteadyStateConfiguration(system)
# Constrain all outputs to zero
for output in outputs:
steady_state_config.ports[output].lower_limit = 0
steady_state_config.ports[output].upper_limit = 0
# Find the steady state
sol = find_steady_state(steady_state_config)
assert sol.success
assert sol.state.size == system.num_states
assert sol.inputs.size == system.num_inputs
npt.assert_allclose(
system.state_derivative(sol.system_state),
np.zeros(system.num_states),
rtol=0,
atol=1e-5,
)
# Set up the configuration for linearization at the steady state
jacobian_config = LinearizationConfiguration(system,
time=0,
state=sol.state,
inputs=sol.inputs)
# Set up the single LTI output
output = OutputDescriptor(jacobian_config, lti.output)
jacobian_config.interpolation_order = interpolation_order
# Linearize the system
A, B, C, D = system_jacobian(jacobian_config)
# Check the matrices
npt.assert_almost_equal(A, np.atleast_2d(lti.system_matrix))
npt.assert_almost_equal(B, np.atleast_2d(lti.input_matrix))
npt.assert_almost_equal(C, np.atleast_2d(lti.output_matrix))
npt.assert_almost_equal(D, np.atleast_2d(lti.feed_through_matrix))
npt.assert_almost_equal(C[output.output_slice],
np.atleast_2d(lti.output_matrix))
npt.assert_almost_equal(D[output.output_slice],
np.atleast_2d(lti.feed_through_matrix))
# Get the full jacobian
jac = system_jacobian(jacobian_config, single_matrix=True)
A = jac[:system.num_states, :system.num_states]
B = jac[:system.num_states, system.num_states:]
C = jac[system.num_states:, :system.num_states]
D = jac[system.num_states:, system.num_states:]
npt.assert_almost_equal(A, np.atleast_2d(lti.system_matrix))
npt.assert_almost_equal(B, np.atleast_2d(lti.input_matrix))
npt.assert_almost_equal(C, np.atleast_2d(lti.output_matrix))
npt.assert_almost_equal(D, np.atleast_2d(lti.feed_through_matrix))
npt.assert_almost_equal(C[output.output_slice],
np.atleast_2d(lti.output_matrix))
npt.assert_almost_equal(D[output.output_slice],
np.atleast_2d(lti.feed_through_matrix))
# Set up the configuration for linearization at default input and state
jacobian_config_def = LinearizationConfiguration(system, time=0)
# Set up the single LTI output
output = OutputDescriptor(jacobian_config_def, lti.output)
jacobian_config_def.interpolation_order = interpolation_order
# Linearize the system, but get the individual matrices
A, B, C, D = system_jacobian(jacobian_config_def)
# Check the matrices
npt.assert_almost_equal(A, np.atleast_2d(lti.system_matrix))
npt.assert_almost_equal(B, np.atleast_2d(lti.input_matrix))
npt.assert_almost_equal(C, np.atleast_2d(lti.output_matrix))
npt.assert_almost_equal(D, np.atleast_2d(lti.feed_through_matrix))
npt.assert_almost_equal(C[output.output_slice],
np.atleast_2d(lti.output_matrix))
npt.assert_almost_equal(D[output.output_slice],
np.atleast_2d(lti.feed_through_matrix))
# Linearize the system, but get the structure
jacobian = system_jacobian(jacobian_config_def, single_matrix="struct")
# Check the matrices
npt.assert_almost_equal(jacobian.system_matrix,
np.atleast_2d(lti.system_matrix))
npt.assert_almost_equal(jacobian.input_matrix,
np.atleast_2d(lti.input_matrix))
npt.assert_almost_equal(jacobian.output_matrix,
np.atleast_2d(lti.output_matrix))
npt.assert_almost_equal(jacobian.feed_through_matrix,
np.atleast_2d(lti.feed_through_matrix))
npt.assert_almost_equal(
jacobian.output_matrix[output.output_slice],
np.atleast_2d(lti.output_matrix),
)
npt.assert_almost_equal(
jacobian.feed_through_matrix[output.output_slice],
np.atleast_2d(lti.feed_through_matrix),
)
steady_state_config.ports[torques_sum].upper_bounds = 0
# Enforce the total vertical force to be zero
# Note that we only force the vertical force to zero, as otherwise we'd have
# too many equality constraints.
# There are the following free variables
# - The four voltage inputs
# - The eight state variables
# There are the following equality constraints
# - The eight state derivatives (which are constrained to zero)
# - The three components of the torque vector
# - The vertical component of the thrust vector
steady_state_config.ports[forces_sum].lower_bounds[2] = 0
steady_state_config.ports[forces_sum].upper_bounds[2] = 0
# Find the steady state of the system
sol = find_steady_state(steady_state_config)
if sol.success:
# The function was successful in finding the steady state
print("Steady state determination was successful")
print("\tstates =%s" % sol.state)
print("\tinputs =%s" % sol.inputs)
# Determine the jacobian of the whole system at the steady state
jacobi_config = LinearizationConfiguration(system,
state=sol.state,
inputs=sol.inputs)
# We want to get the total force and torque as outputs
force_output = OutputDescriptor(jacobi_config, forces_sum)
torque_output = OutputDescriptor(jacobi_config, torques_sum)