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_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_gain_class():
system = System()
gain_block = Gain(system, k=[[1, 2], [3, 4]])
gain_in = constant(value=[3, 4])
gain_block.input.connect(gain_in)
npt.assert_almost_equal(gain_block.output(None), [11, 25])
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_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)
# Create a system
system = System()
# Add a block for 6-DoF (linear and angular motion) of a rigid body
rb_6dof = RigidBody6DOFFlatEarth(system,
mass=MASS,
initial_velocity_earth=[VELOCITY_X, 0, 0],
initial_angular_rates_earth=[0, 0, OMEGA],
moment_of_inertia=moment_of_inertia)
# Use the DirectCosineToEuler block to convert the Direct Cosine Matrix
# provided by the rigid-body block into Euler angles (pitch, roll and yaw)
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
def test_scalar_gain_function():
gain_in = constant(value=[[3, 4], [5, 6]])
gain_signal = gain(gain_matrix=3, input_signal=gain_in)
npt.assert_almost_equal(gain_signal(None), [[9, 12], [15, 18]])
def test_gain_function():
gain_in = constant(value=[3, 4])
gain_signal = gain(gain_matrix=[[1, 2], [3, 4]], input_signal=gain_in)
npt.assert_almost_equal(gain_signal(None), [11, 25])
# Create the system and the engine
system = System()
engine = Engine(
system,
motor_constant=MOTOR_CONSTANT,
resistance=RESISTANCE,
inductance=INDUCTANCE,
moment_of_inertia=MOMENT_OF_INERTIA,
thrust_coefficient=THRUST_COEFFICIENT,
power_coefficient=POWER_COEFFICIENT,
diameter=DIAMETER,
)
# Provide constant signals for the voltage and the air density
voltage = constant(value=3.5)
density = constant(value=1.29)
# Connect them to the corresponding inputs of the engine
engine.voltage.connect(voltage)
engine.density.connect(density)
# 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):
def propeller_model(
thrust_coefficient=0.09,
power_coefficient=0.04,
density=1.29,
diameter=8 * 25.4e-3,
moment_of_inertia=5.284e-6,
target_thrust=1.5 * 9.81 / 4,
maximum_torque=None,
):
"""Create a steady-state configuration for two propellers"""
system = System()
torque_1 = InputSignal(system)
torque_2 = InputSignal(system)
density_signal = constant(density)
propeller_1 = Propeller(
system,
thrust_coefficient=thrust_coefficient,
power_coefficient=power_coefficient,
diameter=diameter,
)
propeller_2 = Propeller(
system,
thrust_coefficient=thrust_coefficient,
power_coefficient=power_coefficient,
diameter=diameter,
)
def speed_1_dt(data):
"""Derivative of the speed of the first propeller"""
tot_torque = torque_1(data) - propeller_1.torque(data)
return tot_torque / (2 * np.pi * moment_of_inertia)
def speed_2_dt(data):
"""Derivative of the speed of the second propeller"""
tot_torque = torque_2(data) - propeller_2.torque(data)
return tot_torque / (2 * np.pi * moment_of_inertia)
speed_1 = State(system, derivative_function=speed_1_dt)
speed_2 = State(system, derivative_function=speed_2_dt)
propeller_1.density.connect(density_signal)
propeller_2.density.connect(density_signal)
propeller_1.speed_rps.connect(speed_1)
propeller_2.speed_rps.connect(speed_2)
total_thrust = sum_signal((propeller_1.thrust, propeller_2.thrust))
total_power = sum_signal((propeller_1.power, propeller_2.power))
# Set up steady-state configuration
config = SteadyStateConfiguration(system)
# Minimize the total power
config.objective = total_power
# Constrain the speed to be positive
config.states[speed_1].lower_bounds = 0
config.states[speed_2].lower_bounds = 0
# Constrain the torques to be positive
config.inputs[torque_1].lower_bounds = 0
config.inputs[torque_2].lower_bounds = 0
npt.assert_equal(config.inputs[torque_1].lower_bounds, 0)
npt.assert_equal(config.inputs[torque_2].lower_bounds, 0)
if maximum_torque is not None:
config.inputs[torque_1].upper_bounds = maximum_torque
config.inputs[torque_2].upper_bounds = maximum_torque
# Constrain the total thrust
config.ports[total_thrust].lower_bounds = target_thrust
config.ports[total_thrust].upper_bounds = target_thrust
return config
]
# Create input and output signals.
# The steady-state determination will try to vary the input signals and states
# such that the state derivatives and the output signals are zero.
# Create individual input signals for the voltages of the four engines
voltages = [
InputSignal(system, value=0),
InputSignal(system, value=0),
InputSignal(system, value=0),
InputSignal(system, value=0),
]
# Provide the air density as input
density = constant(value=1.29)
# Connect the engines
for idx, engine in zip(itertools.count(), engines):
# Provide voltage and density to the engine
voltages[idx].connect(engine.voltage)
density.connect(engine.density)
# We consider gravity and a possible counter torque that need to be compensated
# by thrust and torque
gravity_source = constant(value=np.r_[0, 0, 1.5 * 9.81])
counter_torque = constant(value=np.r_[0, 0, 0])
# Determine the sum of forces and torques
forces_sum = sum_signal([engine.thrust_vector
for engine in engines] + [gravity_source])
def test_system_state():
"""Test the ``SystemState`` class"""
system = System()
input_a = InputSignal(system, shape=(3, 3), value=np.eye(3))
input_c = InputSignal(system, value=1)
input_d = InputSignal(system, shape=2, value=[2, 3])
state_a = State(
system,
shape=(3, 3),
initial_condition=[[1, 2, 3], [4, 5, 6], [7, 8, 9]],
derivative_function=input_a,
)
state_a_dep = State(
system,
shape=(3, 3),
initial_condition=[[1, 2, 3], [4, 5, 6], [7, 8, 9]],
derivative_function=input_a,
)
state_b = State(
system,
shape=3,
initial_condition=[10, 11, 12],
derivative_function=(lambda data: np.r_[13, 14, 15]),
)
state_b1 = State(system, shape=3, derivative_function=state_b)
state_b2 = State(system, shape=3, derivative_function=None)
signal_c = constant(value=16)
event_a = ZeroCrossEventSource(system, event_function=(lambda data: 23))
system_state = SystemState(time=0, system=system)
# Check the initial state
npt.assert_almost_equal(
system_state.state[state_a.state_slice],
state_a.initial_condition.ravel(),
)
npt.assert_almost_equal(
system_state.state[state_a_dep.state_slice],
state_a_dep.initial_condition.ravel(),
)
npt.assert_almost_equal(
system_state.state[state_b.state_slice],
state_b.initial_condition.ravel(),
)
npt.assert_almost_equal(
system_state.state[state_b1.state_slice],
state_b1.initial_condition.ravel(),
)
npt.assert_almost_equal(system_state.state[state_b2.state_slice],
np.zeros(state_b2.size))
# Check the inputs property
npt.assert_almost_equal(system_state.inputs[input_a.input_slice],
np.ravel(input_a.value))
npt.assert_almost_equal(system_state.inputs[input_c.input_slice],
np.ravel(input_c.value))
npt.assert_almost_equal(system_state.inputs[input_d.input_slice],
np.ravel(input_d.value))
# Check the get_state_value function
npt.assert_almost_equal(system_state.get_state_value(state_a),
state_a.initial_condition)
npt.assert_almost_equal(system_state.get_state_value(state_a_dep),
state_a_dep.initial_condition)
npt.assert_almost_equal(system_state.get_state_value(state_b),
state_b.initial_condition)
# Check the function access
npt.assert_equal(event_a(system_state),
event_a.event_function(system_state))
npt.assert_equal(state_a(system_state), state_a.initial_condition)
npt.assert_equal(input_a(system_state), input_a.value)
npt.assert_equal(signal_c(system_state), signal_c.value)