def __init__(self, pin_number):
super().__init__()
self.add(State('high'))
self.add(State('low', is_starting_state=True))
self.add(Transition('high', ['low'], after=lambda: self._change_low))
self.add(Transition('low', ['high'], after=lambda: self._change_high))
self.pin_number = pin_number
def __init__(self, pin_code):
super().__init__()
self.pin_code = pin_code
self.add(State('locked', is_starting_state=True))
self.add(State('unlocked'))
self.add(State('broken'))
self.add(Transition('locked', ['unlocked', 'broken']))
self.add(Transition('unlocked', ['locked', 'broken']))
def __init__(self, up_pin=ACTUATOR_UP_PIN, down_pin=ACTUATOR_DOWN_PIN):
super().__init__()
self.add(State('raised'))
self.add(State('lowered'))
self.add(
State('idle', is_starting_state=True, will_exit=self._localise))
self.add(Transition('idle', ['lowered']))
self.add(Transition('lowered', ['raised'], before=lambda: self._raise))
self.add(Transition('raised', ['lowered'], before=lambda: self._lower))
self.up_pin = up_pin
self.down_pin = down_pin
def __decodeTransitionFromLine(self, line, stateList):
parts = line[:-1].split(",")
states = parts[0].split("-")
if len(parts) > 1:
condition = parts[1]
pass
else:
condition = ""
transition = Transition()
transition.setCondition(condition)
# look for the start states in the state list
for state in stateList:
if state.getName() == states[0]:
transition.setStartState(state)
break
# look for the end states in the state list
for state in stateList:
if state.getName() == states[1]:
transition.setEndState(state)
break
pass
def test_transition(self):
sm = StateMachine()
sm.add(State('online', is_starting_state=True))
sm.add(State('offline'))
sm.add(Transition('online', ['offline']))
sm.transition_to('offline')
def test_new_transition_needs_existing_to_state(self):
sm = StateMachine()
sm.add(State('t'))
with pytest.raises(AssertionError):
t = Transition('f', ['t'])
sm.add(t)
def test_hooks_order(self):
result = []
def hook(s):
def h():
result.append(s)
return h
sm = StateMachine()
sm.add(
State('online',
is_starting_state=True,
will_exit=hook('onwex'),
exited=hook('onex')))
sm.add(State('offline', will_enter=hook('ofwen'),
entered=hook('ofen')))
sm.add(
Transition('online', ['offline'],
before=hook('tb'),
after=hook('ta')))
sm.transition_to('offline')
assert result == ['tb', 'onwex', 'ofwen', 'onex', 'ofen', 'ta']
def test_transitions_arent_two_way(self):
sm = StateMachine()
sm.add(State('online', is_starting_state=True))
sm.add(State('offline'))
sm.add(Transition('offline', ['online']))
assert not sm.can_transition_to('offline')
def __init__(self, stepper):
super().__init__()
self.add(
State('idle',
is_starting_state=True,
will_enter=self.idle,
will_exit=self.localise))
self.add(State('moving'))
self.add(State('stopped'))
self.add(Transition('idle', ['stopped']))
self.add(Transition('stopped', ['moving', 'idle']))
self.add(Transition('moving', ['stopped']))
self.stepper = stepper
self.go_home()
self.idle()
def test_add_Transition(self):
sm = StateMachine()
sm.add(State('f'))
sm.add(State('t'))
t = Transition('f', ['t'])
sm.add(t)
assert sm.transitions == {'f': t}
def create_tr(arm_box_transitions, arm_box_states, form_to):
for indices in form_to:
from_idx, to_idx_tuple = indices
for to_idx in to_idx_tuple:
op_identifier = "m_{}_{}".format(from_idx, to_idx) #
transition = Transition(arm_box_states[from_idx], arm_box_states[to_idx], identifier=op_identifier)
arm_box_transitions[op_identifier] = transition
arm_box_states[from_idx].transitions.append(transition)
return arm_box_transitions, arm_box_states
def createTransitions(self, opLetter):
for indices in self.from_to:
from_idx, to_idx_tuple = indices
for to_idx in to_idx_tuple:
op_identifier = "{}_{}_{}".format(opLetter, from_idx, to_idx)
transition = Transition(self.states[from_idx], self.states[to_idx], identifier=op_identifier)
self.stateTransitions[op_identifier] = transition
self.states[from_idx].transitions.append(transition)
def test_cant_transition_where_cant_transition_to(self):
sm = StateMachine()
sm.add(State('online', is_starting_state=True))
sm.add(State('offline'))
sm.add(Transition('offline', ['online']))
assert not sm.can_transition_to('offline')
with pytest.raises(Exception):
sm.transition_to('offline')
def test_can_transition(self):
sm = StateMachine()
sm.add(State('online', is_starting_state=True))
sm.add(State('offline'))
sm.add(State('error'))
sm.add(Transition('online', ['offline', 'error']))
assert sm.can_transition_to('offline')
assert sm.can_transition_to('error')
def __init__(self):
Config.update()
self.sound = Sound()
# sensor values
self.sensLeft = ColorSensor(INPUT_1)
self.sensRight = ColorSensor(INPUT_4) # TODO: Sensoren anschließen
self.sensIR = InfraredSensor(INPUT_2)
self.sensTouch = TouchSensor(INPUT_3)
self.btn = Button()
self.sensValues = {}
# Classes for features
self.drive = Drive()
self.cross = Cross()
# statemachine
self.fsm = StateMachine()
# adding States
self.fsm.states["followLine"] = State("followLine")
self.fsm.states["followLine"].addFunc(self.drive.followLine,
self.sensValues)
self.fsm.states["brake"] = State("brake")
self.fsm.states["crossFirstTurn"] = State("crossFirstTurn")
self.fsm.states["crossFirstTurn"].addFunc(self.cross.firstTurn,
self.sensValues)
self.fsm.states["checkNextExit"] = State("checkNextExit")
self.fsm.states["checkNextExit"].addFunc(self.drive.followLine,
self.sensValues)
# adding Transitions
self.fsm.transitions["toFollowLine"] = Transition("followLine")
self.fsm.transitions["toBrake"] = Transition("brake")
self.fsm.transitions["toBrake"].addFunc(self.drive.brake)
self.fsm.transitions["toCrossFirstTurn"] = Transition("crossFirstTurn")
def test_cant_transition_without_starting_state(self):
sm = StateMachine()
sm.add(State('online'))
sm.add(State('offline'))
sm.add(State('error'))
sm.add(Transition('online', ['offline', 'error']))
with pytest.raises(Exception):
sm.can_transition_to('offline')
sm.can_transition_to('error')
def transitionsCreate(name, statesmachine, transitionsMachine, fromto):
for indices in fromto:
from_idx, to_idx_tuple = indices # unpack list of two elements into separate from_idx and to_idx_tuple
for to_idx in to_idx_tuple: # iterate over destinations from a source state
op_identifier = name.format(from_idx, to_idx) # parametrize identifier of a transition
# create transition object and add it to the master_transitions dict
transition = Transition(statesmachine[from_idx], statesmachine[to_idx], identifier=op_identifier)
transitionsMachine[op_identifier] = transition
# add transition to source state
statesmachine[from_idx].transitions.append(transition)
def transitions(master_states, form_to):
master_transitions = {}
for indices in form_to:
from_idx, to_idx_tuple = indices # unpack list of two elements into separate from_idx and to_idx_tuple
for to_idx in to_idx_tuple: # iterate over destinations from a source state
op_identifier = "m_{}_{}".format(from_idx, to_idx) # parametrize identifier of a transition
# create transition object and add it to the master_transitions dict
transition = Transition(master_states[from_idx], master_states[to_idx], identifier=op_identifier)
master_transitions[op_identifier] = transition
# add transition to source state
master_states[from_idx].transitions.append(transition)
return master_transitions
def createH():
optionsH = [
{
"name": "A0",
"initial": True,
"value": "a0"
}, # 0
{
"name": "A1",
"initial": False,
"value": "a1"
}, # 1
{
"name": "A2",
"initial": False,
"value": "a2"
}, # 2
{
"name": "A3",
"initial": False,
"value": "a3"
}
] # 3
subordinate_states = [State(**opt) for opt in optionsH]
form_toH = [[0, [1]], [1, [2, 3]], [2, [1]], [3, [0]]]
subordinate_transitions = {}
for indicesH in form_toH:
from_idxH, to_idx_tupleH = indicesH # unpack list of two elements into separate from_idx and to_idx_tuple
for to_idxH in to_idx_tupleH: # iterate over destinations from a source state
op_identifierH = "k_{}_{}".format(
from_idxH, to_idxH) # parametrize identifier of a transition
# create transition object and add it to the master_transitions dict
transitionH = Transition(subordinate_states[from_idxH],
subordinate_states[to_idxH],
identifier=op_identifierH)
subordinate_transitions[op_identifierH] = transitionH
# add transition to source state
subordinate_states[from_idxH].transitions.append(transitionH)
subordinate = Generator.create_master(subordinate_states,
subordinate_transitions)
return subordinate, subordinate_transitions
def test_from_state_check(self):
assert 'from' not in Transition('from', ['to'])
def test_string_representation(self):
t = Transition('a', ['b', 'c'])
assert str(t) == "a -> b, c"
def test_standard_construction(self):
t = Transition('a', ['b', 'c'])
assert t.from_state_name == 'a'
assert t.to_states_names == ['b', 'c']
def test_value_is_contained(self):
assert 'a1' in Transition('a', ['a1', 'b1', 'c1'])
assert 'b1' in Transition('a', ['a1', 'b1', 'c1'])
assert 'c1' in Transition('a', ['a1', 'b1', 'c1'])
def test_both_hooks(self):
t = Transition('a', ['b'],
before=self.sample_hook(4),
after=self.sample_hook(5))
assert t.before() == 4
assert t.after() == 5
def test_after(self):
t = Transition('a', ['b'], after=self.sample_hook(4))
assert t.after() == 4
assert t.before() == None
def __init__(self):
# define states for a master (way of passing args to class)
options1 = [
{
"name": "Stacja zgrzewająca pusta",
"initial": True,
"value": "Station_empty"
}, # 0
{
"name": "Stacja czeka na komplet butelek",
"initial": False,
"value": "Station_waiting"
}, # 1
{
"name": "Stacja wysłała sygnał i czeka na podanie butelki",
"initial": False,
"value": "Station_incomplete"
}, # 2
{
"name": "Stacja ma zestaw butelek",
"initial": False,
"value": "Station_ready"
}, # 3
{
"name": "Stacja ma zgrzewkę",
"initial": False,
"value": "Station_done"
}, # 4
{
"name": "Awaria stacji",
"initial": False,
"value": "Station_failure"
}
] # 5
options2 = [
{
"name": "Manipulator w pozycji bazowej",
"initial": True,
"value": "Robot_in_base_position"
}, # 0
{
"name": "Manipulator w pozycji gotowej do pochwycenia",
"initial": False,
"value": "Robot_ready"
}, # 1
{
"name": "Manipulator trzymający butelkę",
"initial": False,
"value": "Robot_holding_beer"
}, # 2
{
"name": "Manipulator w pozycji końcowej",
"initial": False,
"value": "Robot_in_final_position"
}, # 3
{
"name": "Powrót do procesu zgrzewania",
"initial": False,
"value": "Return_to_shrink_process"
} # 4
]
options3 = [
{
"name": "Operator oczekuje na sygnał błędu",
"initial": True,
"value": "Error_signal_received"
}, # 0
{
"name": "Sygnalizacja błędu",
"initial": False,
"value": "Error_signalized"
}, # 1
{
"name": "Usunięcie zgrzewki wody",
"initial": False,
"value": "Employee_reaction"
}, # 2
{
"name": "Powrót do procesu",
"initial": False,
"value": "Return_to_process"
}
] # 3
# create State objects for a master
# ** -> unpack dict to args
self.master_states = [State(**opt) for opt in options1]
self.slave1_states = [State(**opt) for opt in options2]
self.slave2_states = [State(**opt) for opt in options3]
# valid transitions for a master (indices of states from-to)
self.from_to1 = [[0, [1]], [1, [2, 3]], [2, [1]], [3, [4]],
[4, [5, 0]], [5, [0]]]
self.from_to2 = [[0, [1]], [1, [2]], [2, [3]], [3, [4]], [4, []]]
self.from_to3 = [[0, [1]], [1, [2]], [2, [3]], [3, []]]
# create transitions dict
transitions = {"master": {}, "slave1": {}, "slave2": {}}
# machines dict for a states and transitions creating
machines = {
"master": {
"transitions": transitions["master"],
"from_to": self.from_to1,
"id": "m_{}_{}",
"states": self.master_states
},
"slave1": {
"transitions": transitions["slave1"],
"from_to": self.from_to2,
"id": "s1_{}_{}",
"states": self.slave1_states
},
"slave2": {
"transitions": transitions["slave2"],
"from_to": self.from_to3,
"id": "s2_{}_{}",
"states": self.slave2_states
}
}
for key in machines:
for indices in machines[key]["from_to"]:
from_idx, to_idx_tuple = indices # unpack list of two elements into separate from_idx and to_idx_tuple
for to_idx in to_idx_tuple: # iterate over destinations from a source state
op_identifier = machines[key]["id"].format(
from_idx,
to_idx) # parametrize identifier of a transition
# create transition object and add it to the master_transitions dict
transition = Transition(machines[key]["states"][from_idx],
machines[key]["states"][to_idx],
identifier=op_identifier)
machines[key]["transitions"][op_identifier] = transition
# add transition to source state
machines[key]["states"][from_idx].transitions.append(
transition)
# save states and transitions
self.transitions = transitions
self.states = {
"master": self.master_states,
"slave1": self.slave1_states,
"slave2": self.slave2_states
}
self.current_machine = None
self.machines = {}
def test_value_not_contained(self):
assert 'not_there' not in Transition('a', ['b'])
def test_empty_hooks(self):
t = Transition('a', ['b'])
assert t.before() == None
assert t.after() == None
[6, [0]]]
form_to_R1 = [[0, [1]], [1, [2, 3]], [2, [3, 0]], [3, [0]]]
form_to_R2 = [[0, [1]], [1, [2, 3]], [2, [3, 0]], [3, [0]]]
# create transitions for a master (as a dict)
master_transitions = {}
for indices in form_to:
from_idx, to_idx_tuple = indices # unpack list of two elements into separate from_idx and to_idx_tuple
for to_idx in to_idx_tuple: # iterate over destinations from a source state
op_identifier = "m_{}_{}".format(
from_idx, to_idx) # parametrize identifier of a transition
# create transition object and add it to the master_transitions dict
transition = Transition(st.master_states[from_idx],
st.master_states[to_idx],
identifier=op_identifier)
master_transitions[op_identifier] = transition
# add transition to source state
st.master_states[from_idx].transitions.append(transition)
# create transitions for a robot1 (as a dict)
robotL_transitions = {}
for indices in form_to_R1:
from_idx, to_idx_tuple = indices # unpack list of two elements into separate from_idx and to_idx_tuple
for to_idx in to_idx_tuple: # iterate over destinations from a source state
op_identifier = "r1_{}_{}".format(
from_idx, to_idx) # parametrize identifier of a transition
# create transition object and add it to the master_transitions dict
def run():
master_states = [State(**opt) for opt in options]
form_to = [[0, [1, 9]], [1, [0, 2, 9]], [2, [3, 9]], [3, [4, 9]],
[4, [1, 5, 6, 7, 9]], [5, [1, 9]], [6, [8, 9]], [7, [8, 9]],
[8, [1, 9]], [9, [10]], [10, [0, 1, 2, 3, 4, 5, 6, 7, 8]]]
master_transitions = {}
for indices in form_to:
from_idx, to_idx_tuple = indices
for to_idx in to_idx_tuple:
op_identifier = "m_{}_{}".format(from_idx, to_idx)
transition = Transition(master_states[from_idx],
master_states[to_idx],
identifier=op_identifier)
master_transitions[op_identifier] = transition
master_states[from_idx].transitions.append(transition)
input = input_sequence
paths = [path_bases[key] for key in input]
init_state = "idle"
path_array = []
start = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
previous_position = start
for path in paths:
supervisor = Generator.create_master(master_states, master_transitions)
print('\n' + str(supervisor))
print("Executing path: {}".format(path))
for event in path:
master_transitions[event]._run(supervisor)
print(supervisor.current_state)
if supervisor.current_state.value == "idle":
path1 = move_j(robot, previous_position, start)
previous_position = start
path_array.append(path1)
print("Supervisor done!")
if supervisor.current_state.value == "scan":
path1 = move_j(robot, previous_position, scan)
previous_position = scan
path_array.append(path1)
if supervisor.current_state.value == "grip":
path1 = move_j(robot, previous_position, grip)
previous_position = grip
path_array.append(path1)
if supervisor.current_state.value == "evaluate":
path1 = move_j(robot, previous_position, evaluate)
previous_position = evaluate
path_array.append(path1)
if supervisor.current_state.value == "trash":
path1 = move_j(robot, previous_position, trash)
previous_position = trash
path_array.append(path1)
if supervisor.current_state.value == "transport_a":
path1 = move_j(robot, previous_position, transport_a)
previous_position = transport_a
path_array.append(path1)
if supervisor.current_state.value == "transport_b":
path1 = move_j(robot, previous_position, transport_b)
previous_position = transport_b
path_array.append(path1)
if supervisor.current_state.value == "detach":
if previous_position == trash:
path1 = move_j(robot, previous_position, detach)
previous_position = detach
elif previous_position == transport_a:
path1 = move_j(robot, previous_position, detach_a)
previous_position = detach_a
elif previous_position == transport_b:
path1 = move_j(robot, previous_position, detach_b)
previous_position = detach_b
path_array.append(path1)
if random.randint(0, 100) > 90:
idx = options_idx[supervisor.current_state.value]
master_transitions[f'm_{idx}_9']._run(supervisor)
print(supervisor.current_state)
master_transitions['m_9_10']._run(supervisor)
print(supervisor.current_state)
print("Monkeys are repairing machine...")
n = 100
for i in range(n):
if i % 10 == 0:
print("Repairing machine ({}%)".format(100 * i // n))
time.sleep(0.01)
master_transitions[f'm_{10}_{idx}']._run(supervisor)
print("Recovered from fatal crash!")
print(supervisor.current_state)
path = np.concatenate(path_array, axis=0)
model.animate(stances=path, frame_rate=30, unit='deg')
