def __init__(self, name, num_rtol, num_atol, START_STEP_SIZE,
FMU_START_RATE):
self._num_rtol = num_rtol
self._num_atol = num_atol
self._max_iterations = 100
self._power = PowerFMU("power",
num_rtol,
num_atol,
START_STEP_SIZE / FMU_START_RATE,
J=0.085,
b=5,
K=7.45,
R=0.15,
L=0.036,
V_a=12)
self.omega = self._power.omega
self.theta = self._power.theta
self._window = WindowFMU("window",
num_rtol,
num_atol,
START_STEP_SIZE / FMU_START_RATE,
r=0.11,
b=10)
self._obstacle = ObstacleFMU("obstacle",
num_rtol,
num_atol,
START_STEP_SIZE / FMU_START_RATE,
c=1e5,
fixed_x=0.45)
self.up = self._power.up
self.down = self._power.down
input_vars = [self.down, self.up]
self.i = self._power.i
self.omega = self._power.omega
self.theta = self._power.theta
self.x = self._window.x
self.F = self._obstacle.F
state_vars = [self.i, self.omega, self.theta, self.x, self.F]
algebraic_functions = {}
AbstractSimulationUnit.__init__(self, name, algebraic_functions,
state_vars, input_vars)
V_a=12)
armature_threshold = 5
adapt_armature = InacurateControllerArmatureAdaptation_CT(
"arm_adapt", NUM_RTOL, NUM_ATOL, armature_threshold, True)
window = WindowFMU("window",
NUM_RTOL,
NUM_ATOL,
START_STEP_SIZE / FMU_START_RATE,
r=0.11,
b=10)
obstacle = ObstacleFMU("obstacle",
NUM_RTOL,
NUM_ATOL,
START_STEP_SIZE / FMU_START_RATE,
c=1e5,
fixed_x=0.45)
environment.enterInitMode()
controller.enterInitMode()
power.enterInitMode()
adapt_armature.enterInitMode()
window.enterInitMode()
obstacle.enterInitMode()
step = 0
iteration = 0
# Set environment initial inputs
# Nothing to do
armature_threshold = 5 # This was the initial threshold, but did not work.
adapt_armature = AccurateControllerArmatureAdaptation("arm_adapt", NUM_RTOL,
NUM_ATOL,
armature_threshold, True)
window = WindowFMU("window",
NUM_RTOL,
NUM_ATOL,
START_STEP_SIZE / FMU_START_RATE,
r=0.11,
b=10)
obstacle = ObstacleFMU("obstacle",
NUM_RTOL,
NUM_ATOL,
START_STEP_SIZE / FMU_START_RATE,
c=1e5,
fixed_x=0.45)
delay_up = DecoupleAdaptation("delay_up")
delay_down = DecoupleAdaptation("delay_down")
delay_reaction_torque = DecoupleAdaptation("delay_reaction_torque")
delay_height = DecoupleAdaptation("delay_height")
units = [
environment, # 0
controller, # 1
power, # 2
adapt_armature, # 3
window, # 4
obstacle, # 5
class AlgebraicAdaptation_Power_Window_Obstacle(AbstractSimulationUnit):
"""
This is the adaptation that realizes the strong coupling between the power,
window, and obstacle units.
"""
def __init__(self, name, num_rtol, num_atol, START_STEP_SIZE,
FMU_START_RATE):
self._num_rtol = num_rtol
self._num_atol = num_atol
self._max_iterations = 100
self._power = PowerFMU("power",
num_rtol,
num_atol,
START_STEP_SIZE / FMU_START_RATE,
J=0.085,
b=5,
K=7.45,
R=0.15,
L=0.036,
V_a=12)
self.omega = self._power.omega
self.theta = self._power.theta
self._window = WindowFMU("window",
num_rtol,
num_atol,
START_STEP_SIZE / FMU_START_RATE,
r=0.11,
b=10)
self._obstacle = ObstacleFMU("obstacle",
num_rtol,
num_atol,
START_STEP_SIZE / FMU_START_RATE,
c=1e5,
fixed_x=0.45)
self.up = self._power.up
self.down = self._power.down
input_vars = [self.down, self.up]
self.i = self._power.i
self.omega = self._power.omega
self.theta = self._power.theta
self.x = self._window.x
self.F = self._obstacle.F
state_vars = [self.i, self.omega, self.theta, self.x, self.F]
algebraic_functions = {}
AbstractSimulationUnit.__init__(self, name, algebraic_functions,
state_vars, input_vars)
def _isClose(self, a, b):
return numpy.isclose(a, b, self._num_rtol, self._num_atol)
def _biggerThan(self, a, b):
return not numpy.isclose(a, b, self._num_rtol,
self._num_atol) and a > b
def _doInternalSteps(self, time, step, iteration, step_size):
l.debug(">%s._doInternalSteps(%f, %d, %d, %f)", self._name, time, step,
iteration, step_size)
assert step_size > 0.0, "step_size too small: {0}".format(step_size)
#assert self._biggerThan(step_size, 0), "step_size too small: {0}".format(step_size)
assert iteration == 0, "Fixed point iterations not supported outside of this component."
converged = False
internal_iteration = 0
cOut = self.getValues(step, iteration, [self.up, self.down])
wOut = self._window.getValues(step - 1, iteration,
[self._window.x, self._window.tau])
pOut = None
while not converged and internal_iteration < self._max_iterations:
self._power.setValues(
step,
iteration,
{
self._power.tau: wOut[self._window.tau], # Delayed input
self._power.up: cOut[self.up],
self._power.down: cOut[self.down]
})
self._power.doStep(time, step, iteration, step_size)
pOut = self._power.getValues(
step, iteration,
[self._power.omega, self._power.theta, self._power.i])
self._obstacle.setValues(
step, iteration,
{self._obstacle.x: wOut[self._window.x]}) # Delayed input
self._obstacle.doStep(time, step, iteration, step_size)
oOut = self._obstacle.getValues(step, iteration,
[self._obstacle.F])
self._window.setValues(
step, iteration, {
self._window.omega_input: pOut[self._power.omega],
self._window.theta_input: pOut[self._power.theta],
self._window.F_obj: oOut[self._obstacle.F]
})
self._window.doStep(time, step, iteration, step_size)
wOut_corrected = self._window.getValues(
step, iteration, [self._window.x, self._window.tau])
l.debug("Iteration step completed:")
l.debug("wOut=%s;", wOut)
l.debug("wOut_corrected=%s.", wOut_corrected)
if self._isClose(wOut[self._window.x], wOut_corrected[self._window.x]) \
and self._isClose(wOut[self._window.tau], wOut_corrected[self._window.tau]):
converged = True
internal_iteration = internal_iteration + 1
wOut = wOut_corrected
if converged:
l.debug("Fixed point found after %d iterations",
internal_iteration)
else:
l.debug("Fixed point not found after %d iterations",
internal_iteration)
raise RuntimeError("Fixed point not found")
AbstractSimulationUnit.setValues(
self, step, iteration, {
self.i: pOut[self._power.i],
self.theta: pOut[self._power.theta],
self.omega: pOut[self._power.omega],
self.x: wOut[self._window.x],
self.F: oOut[self._obstacle.F]
})
l.debug("<%s._doInternalSteps() = (%s, %d)", self._name, STEP_ACCEPT,
step_size)
return (STEP_ACCEPT, step_size)
def setValues(self, step, iteration, values):
l.debug(
">%s.AlgebraicAdaptation_Power_Window_Obstacle.setValues(%d, %d, %s)",
self._name, step, iteration, values)
# Filter just the inputs.
inputs = {self.up: values[self.up], self.down: values[self.down]}
AbstractSimulationUnit.setValues(self, step, iteration, inputs)
if self._mode == INIT_MODE:
# Initialize the internal FMUs and compute the value of the armature.
l.debug("Initializing strong component...")
step = iteration = 0
wOut_tau_delayed = 0.0
wOut_x_delayed = 0.0
# Set power inputs (or initial state, given by values)
self._power.setValues(step, iteration, values)
self._power.setValues(step, iteration,
{self._power.tau: wOut_tau_delayed})
# Get power initial outputs
pOut = self._power.getValues(
step, iteration,
[self._power.omega, self._power.theta, self._power.i])
# Set obstacle initial inputs
# Assume they are zero because the outputs of the window are delayed.
self._obstacle.setValues(step, iteration,
{self._obstacle.x: wOut_x_delayed})
# Get obstacle outputs
oOut = self._obstacle.getValues(step, iteration,
[self._obstacle.F])
# Set window inputs
self._window.setValues(
step, iteration, {
self._window.omega_input: pOut[self._power.omega],
self._window.theta_input: pOut[self._power.theta],
self._window.F_obj: oOut[self._obstacle.F]
})
# Get window outputs
wOut = self._window.getValues(step, iteration,
[self._window.x, self._window.tau])
# Set corrected power windows
self._power.setValues(
step,
iteration,
{
self._power.tau:
wOut[self._window.tau] # Delayed input from window
})
# We know that convergence is easily achieved for this initialisation
assert self._isClose(wOut[self._window.tau], wOut_tau_delayed)
assert self._isClose(wOut[self._window.x], wOut_x_delayed)
# Record the outputs
AbstractSimulationUnit.setValues(
self, step, iteration, {
self.i: pOut[self._power.i],
self.theta: pOut[self._power.theta],
self.omega: pOut[self._power.omega],
self.x: wOut[self._window.x],
self.F: oOut[self._obstacle.F]
})
l.debug("Strong component initialized.")
l.debug("<%s.AlgebraicAdaptation_Power_Window_Obstacle.setValues()",
self._name)
def enterInitMode(self):
l.debug(
">%s.AlgebraicAdaptation_Power_Window_Obstacle.enterInitMode()",
self._name)
AbstractSimulationUnit.enterInitMode(self)
self._power.enterInitMode()
self._window.enterInitMode()
self._obstacle.enterInitMode()
l.debug(
"<%s.AlgebraicAdaptation_Power_Window_Obstacle.enterInitMode()",
self._name)
def exitInitMode(self):
l.debug(">%s.AlgebraicAdaptation_Power_Window_Obstacle.exitInitMode()",
self._name)
AbstractSimulationUnit.exitInitMode(self)
self._power.exitInitMode()
self._window.exitInitMode()
self._obstacle.exitInitMode()
l.debug("<%s.AlgebraicAdaptation_Power_Window_Obstacle.exitInitMode()",
self._name)
def __init__(self, name, num_rtol, num_atol, start_rate, start_step_size):
self.in_power_up = "in_power_up"
self.in_power_down = "in_power_down"
self.out_power_i = "out_power_i"
self.out_window_x = "out_window_x"
self.out_obs_F = "out_obs_F"
power = PowerFMU(name + ":" + "power",
num_rtol,
num_atol,
start_step_size / start_rate,
J=0.085,
b=5,
K=7.45,
R=0.15,
L=0.036,
V_a=12)
window = WindowFMU(name + ":" + "window",
num_rtol,
num_atol,
start_step_size / start_rate,
r=0.11,
b=10)
obstacle = ObstacleFMU(name + ":" + "obstacle",
num_rtol,
num_atol,
start_step_size / start_rate,
c=1e5,
fixed_x=0.45)
delay_reaction_torque = DecoupleAdaptation(name + ":" +
"delay_reaction_torque")
delay_height = DecoupleAdaptation(name + ":" + "delay_height")
zoh_up = ZOHAdaptation(name + ":" + "zoh_up", "in_ext_up",
"out_ext_up")
zoh_down = ZOHAdaptation(name + ":" + "zoh_down", "in_ext_down",
"out_ext_down")
units = [
power, # 0
window, # 1
obstacle, # 2
delay_height, # 3
delay_reaction_torque, # 4
zoh_up, # 5
zoh_down # 6
]
order = [5, 6, 3, 4, 2, 0, 1]
initial_state = [{
power.omega: 0.0,
power.theta: 0.0,
power.i: 0.0
}, None, None, {
delay_height.out_v: 0.0
}, {
delay_reaction_torque.out_v: 0.0
}, None, None]
def coupling_power(y):
zohUpOut = y[5]
zohDownOut = y[6]
dtauOut = y[4]
return {
power.up: zohUpOut[zoh_up.output_var],
power.down: zohDownOut[zoh_down.output_var],
power.tau: dtauOut[delay_reaction_torque.out_v]
}
def coupling_window(y):
pOut = y[0]
oOut = y[2]
return {
window.omega_input: pOut[power.omega],
window.theta_input: pOut[power.theta],
window.F_obj: oOut[obstacle.F]
}
def coupling_obstacle(y):
dheightOut = y[3]
return {obstacle.x: dheightOut[delay_height.out_v]}
def coupling_delay_torque(y):
wOut = y[1]
return {
delay_reaction_torque.in_v: wOut[window.tau]
} if wOut != None else None
def coupling_delay_height(y):
wOut = y[1]
return {
delay_height.in_v: wOut[window.x]
} if wOut != None else None
def coupling_zoh_up(y):
last_known_inputs = self._inputs
output = None
if last_known_inputs != None:
output = {
zoh_up.input_var: last_known_inputs[self.in_power_up]
}
return output
def coupling_zoh_down(y):
last_known_inputs = self._inputs
output = None
if last_known_inputs != None:
output = {
zoh_down.input_var: last_known_inputs[self.in_power_down]
}
return output
def external_coupling(y):
pOut = y[0]
wOut = y[1]
oOut = y[2]
return {
self.out_power_i: pOut[power.i],
self.out_window_x: wOut[window.x],
self.out_obs_F: oOut[obstacle.F]
}
couplings = [
coupling_power, coupling_window, coupling_obstacle,
coupling_delay_height, coupling_delay_torque, coupling_zoh_up,
coupling_zoh_down
]
MultiRateAdaptationUnit.__init__(self, name, num_rtol, num_atol, order,
units, couplings, external_coupling,
start_rate, initial_state)