def setUp(self):
l1 = LeafCombination(
CollaborativeAction('Do a', (3., 2., 5.), human_probability=.3))
l2 = LeafCombination(
CollaborativeAction('Do b', (2., 3., 4.), human_probability=.7))
alt = AlternativeCombination([l1, l2], name='Do all')
self.n2p = _NodeToPOMDP.from_node(alt, 2., 1.)
def setUp(self):
a = CollaborativeAction('Do a', (3., 2., 5.))
b = CollaborativeAction('Do b', (2., 3., 4.))
c = CollaborativeAction('Do c', (2., 3., 4.))
d = CollaborativeAction('Do d', (3., 2., 5.))
self.l1 = LeafCombination(a)
self.l2 = LeafCombination(b)
self.l3 = LeafCombination(c)
self.l4 = LeafCombination(d)
def test_end_reward_on_seq(self):
h2p = HTMToPOMDP(1., 2., 1., 1., end_reward=13.)
task = HierarchicalTask(root=SequentialCombination([
LeafCombination(
CollaborativeAction('Do a', (3., 2., 5.),
fail_probability=0.,
no_probability=0.)),
LeafCombination(
CollaborativeAction('Do b', (2., 3., 4.),
fail_probability=0.,
no_probability=0.)),
],
name='Do all'))
p = h2p.task_to_pomdp(task)
self.assertTrue((p.R[h2p.wait, h2p.end, h2p.end, :] == 13.).all())
def test_with_no_probability(self):
leaf = LeafCombination(
CollaborativeAction('Do it', (3., 2., 5.), no_probability=.2))
l2p = _NodeToPOMDP.from_node(leaf, 2., 1.)
T = np.zeros((7, 8, 8))
l2p.update_T(T, 0, 2, 3, [6, 7], [.2, .8], list(range(7)))
p_ask_int = np.zeros((8))
p_ask_int[4] = .8
p_ask_int[5] = .2
np.testing.assert_allclose(T[3, 3, :], p_ask_int)
def test_with_failure_probability(self):
leaf = LeafCombination(
CollaborativeAction('Do it', (3., 2., 5.), fail_probability=.2))
l2p = _NodeToPOMDP.from_node(leaf, 2., 1.)
T = np.zeros((7, 8, 8))
l2p.update_T(T, 0, 2, 3, [6, 7], [.2, .8], list(range(7)))
p_phy_robot = np.zeros((8))
p_phy_robot[5] = .2
p_phy_robot[6] = .8 * .2
p_phy_robot[7] = .8 * .8
np.testing.assert_allclose(T[2, 5, :], p_phy_robot)
def test_end_reward(self):
h2p = HTMToPOMDP(1., 2., 1., 1., end_reward=13.)
task = HierarchicalTask(
root=LeafCombination(CollaborativeAction('Do it', (3., 2., 5.))))
p = h2p.task_to_pomdp(task)
self.assertTrue((p.R[h2p.wait, h2p.end, h2p.end, :] == 13.).all())
def test_alt_to_pomdp(self):
# No probability of failure or human saying no here
task = HierarchicalTask(root=AlternativeCombination([
LeafCombination(
CollaborativeAction('Do a', (3., 2., 5.),
fail_probability=0.,
no_probability=0.)),
LeafCombination(
CollaborativeAction('Do b', (2., 3., 4.),
fail_probability=0.,
no_probability=0.)),
],
name='Do either'))
p = self.h2p.task_to_pomdp(task)
self.assertEqual(p.states, [
'init-do-a', 'H-do-a', 'R-do-a', 'init-do-b', 'H-do-b', 'R-do-b',
'end'
])
self.assertEqual(p.actions, [
'wait', 'phy-do-a', 'com-ask-intention-do-a',
'com-tell-intention-do-a', 'com-ask-finished-do-a', 'phy-do-b',
'com-ask-intention-do-b', 'com-tell-intention-do-b',
'com-ask-finished-do-b'
])
self.assertEqual(p.observations, ['none', 'yes', 'no', 'error'])
np.testing.assert_array_equal(p.start,
np.array([.5, 0, 0, .5, 0, 0, 0]))
# checked manually:
T = np.array([
# Wait
[[1., 0., 0., 0., 0., 0., 0.],
[0., 0.71653, 0., 0., 0., 0., 0.28347],
[0., 0., 1., 0., 0., 0., 0.], [0., 0., 0., 1., 0., 0., 0.],
[0., 0., 0., 0., 0.60653, 0., 0.39347],
[0., 0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 0., 1.]],
# Physical a
[[1., 0., 0., 0., 0., 0., 0.],
[0., 0.18888, 0., 0., 0., 0., 0.81112],
[0., 0., 0., 0., 0., 0., 1.], [0., 0., 0., 1., 0., 0., 0.],
[0., 0., 0., 0., 0.08208, 0., 0.91792],
[0., 0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 0., 1.]],
# Ask intention a
[[0., 1., 0., 0., 0., 0., 0.],
[0., 0.51342, 0., 0., 0., 0., 0.48658],
[0., 0., 1., 0., 0., 0., 0.], [0., 0., 0., 1., 0., 0., 0.],
[0., 0., 0., 0., 0.36788, 0., 0.63212],
[0., 0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 0., 1.]],
# Tell intention a
[[0., 0., 1., 0., 0., 0., 0.],
[0., 0.71653, 0., 0., 0., 0., 0.28347],
[0., 0., 1., 0., 0., 0., 0.], [0., 0., 0., 1., 0., 0., 0.],
[0., 0., 0., 0., 0.60653, 0., 0.39347],
[0., 0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 0., 1.]],
# Ask finished a
[[1., 0., 0., 0., 0., 0., 0.],
[0., 0.51342, 0., 0., 0., 0., 0.48658],
[0., 0., 1., 0., 0., 0., 0.], [0., 0., 0., 1., 0., 0., 0.],
[0., 0., 0., 0., 0.36788, 0., 0.63212],
[0., 0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 0., 1.]],
# Physical b
[[1., 0., 0., 0., 0., 0.,
0.], [0., 0.2636, 0., 0., 0., 0., 0.7364],
[0., 0., 1., 0., 0., 0., 0.], [0., 0., 0., 1., 0., 0., 0.],
[0., 0., 0., 0., 0.13534, 0., 0.86466],
[0., 0., 0., 0., 0., 0., 1.], [0., 0., 0., 0., 0., 0., 1.]],
# Ask intention b
[[1., 0., 0., 0., 0., 0., 0.],
[0., 0.51342, 0., 0., 0., 0., 0.48658],
[0., 0., 1., 0., 0., 0., 0.], [0., 0., 0., 0., 1., 0., 0.],
[0., 0., 0., 0., 0.36788, 0., 0.63212],
[0., 0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 0., 1.]],
# Tell intention b
[[1., 0., 0., 0., 0., 0., 0.],
[0., 0.71653, 0., 0., 0., 0., 0.28347],
[0., 0., 1., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 1., 0.],
[0., 0., 0., 0., 0.60653, 0., 0.39347],
[0., 0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 0., 1.]],
# Ask finished b
[[1., 0., 0., 0., 0., 0., 0.],
[0., 0.51342, 0., 0., 0., 0., 0.48658],
[0., 0., 1., 0., 0., 0., 0.], [0., 0., 0., 1., 0., 0., 0.],
[0., 0., 0., 0., 0.36788, 0., 0.63212],
[0., 0., 0., 0., 0., 1., 0.], [0., 0., 0., 0., 0., 0., 1.]],
])
np.testing.assert_allclose(T, p.T, atol=1.e-4)
O = np.array([
# Wait
[[1., 0., 0., 0.], [1., 0., 0., 0.], [1., 0., 0., 0.],
[1., 0., 0., 0.], [1., 0., 0., 0.], [1., 0., 0., 0.],
[1., 0., 0., 0.]],
# Physical a
[[0., 0., 0., 1.], [0., 0., 0., 1.], [0., 0., 0., 1.],
[0., 0., 0., 1.], [0., 0., 0., 1.], [0., 0., 0., 1.],
[1., 0., 0., 0.]],
# Ask intention a
[[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.],
[1., 0., 0., 0.], [1., 0., 0., 0.], [1., 0., 0., 0.],
[1., 0., 0., 0.]],
# Tell intention a
[[1., 0., 0., 0.], [1., 0., 0., 0.], [1., 0., 0., 0.],
[1., 0., 0., 0.], [1., 0., 0., 0.], [1., 0., 0., 0.],
[1., 0., 0., 0.]],
# Ask finished a
[[0., 0., 1., 0.], [0., 0., 1., 0.], [0., 0., 1., 0.],
[0., 0., 1., 0.], [0., 0., 1., 0.], [0., 0., 1., 0.],
[0., 1., 0., 0.]],
# Physical b
[[0., 0., 0., 1.], [0., 0., 0., 1.], [0., 0., 0., 1.],
[0., 0., 0., 1.], [0., 0., 0., 1.], [0., 0., 0., 1.],
[1., 0., 0., 0.]],
# Ask intention
[[1., 0., 0., 0.], [1., 0., 0., 0.], [1., 0., 0., 0.],
[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.],
[1., 0., 0., 0.]],
# Tell intention
[[1., 0., 0., 0.], [1., 0., 0., 0.], [1., 0., 0., 0.],
[1., 0., 0., 0.], [1., 0., 0., 0.], [1., 0., 0., 0.],
[1., 0., 0., 0.]],
# Ask finished b
[[0., 0., 1., 0.], [0., 0., 1., 0.], [0., 0., 1., 0.],
[0., 0., 1., 0.], [0., 0., 1., 0.], [0., 0., 1., 0.],
[0., 1., 0., 0.]],
])
np.testing.assert_array_equal(O, p.O)
R = -np.broadcast_to(
np.array([[1] * 6 + [0], [6, 6, 3, 6, 6, 6, 1],
[3] * 6 + [1], [2] * 6 + [1], [3] * 6 + [1],
[5, 5, 5, 5, 5, 4, 1], [3] * 6 + [1], [2] * 6 + [1],
[3] * 6 + [1]])[:, :, None, None], (9, 7, 7, 4))
np.testing.assert_array_equal(R, p.R)
def test_leaf_to_pomdp(self):
# No probability of failure or human saying no here
task = HierarchicalTask(root=LeafCombination(
CollaborativeAction(
'Do it', (3., 2.,
5.), fail_probability=0., no_probability=0.)))
p = self.h2p.task_to_pomdp(task)
self.assertEqual(p.states, ['init-do-it', 'H-do-it', 'R-do-it', 'end'])
self.assertEqual(p.actions, [
'wait', 'phy-do-it', 'com-ask-intention-do-it',
'com-tell-intention-do-it', 'com-ask-finished-do-it'
])
self.assertEqual(p.observations, ['none', 'yes', 'no', 'error'])
np.testing.assert_array_equal(p.start, np.array([1, 0., 0., 0.]))
# checked manually:
T = np.array([
[[1., 0., 0., 0.], [0., 0.71653131, 0., 0.28346869],
[0., 0., 1., 0.], [0., 0., 0., 1.]],
[[1., 0., 0., 0.], [0., 0.1888756, 0., 0.8111244],
[0., 0., 0., 1.], [0., 0., 0., 1.]],
[[0., 1., 0., 0.], [0., 0.51341712, 0., 0.48658288],
[0., 0., 1., 0.], [0., 0., 0., 1.]],
[[0., 0., 1., 0.], [0., 0.71653131, 0., 0.28346869],
[0., 0., 1., 0.], [0., 0., 0., 1.]],
[[1., 0., 0., 0.], [0., 0.51341712, 0., 0.48658288],
[0., 0., 1., 0.], [0., 0., 0., 1.]],
])
np.testing.assert_allclose(T, p.T)
O = np.array([
# Wait
[[1., 0., 0., 0.], [1., 0., 0., 0.], [1., 0., 0., 0.],
[1., 0., 0., 0.]],
# Act
[[0., 0., 0., 1.], [0., 0., 0., 1.], [0., 0., 0., 1.],
[1., 0., 0., 0.]],
# Ask intention
[
[1., 0., 0., 0.], # not possible in T
[0., 1., 0., 0.],
[0., 0., 1., 0.], # robot has told its int. to act
[1., 0., 0., 0.]
], # human acts while robot ask again
# # TODO: change to Yes?
# Tell intention
[
[1., 0., 0., 0.], # not possible in T
[1., 0., 0., 0.], # TODO: No?
[1., 0., 0., 0.], # TODO: maybe H answers to R tell
[1., 0., 0., 0.]
], # TODO: answer?
# Ask finished
[
[0., 0., 1., 0.], # not started
[0., 0., 1., 0.],
[0., 0., 1., 0.],
[0., 1., 0., 0.]
],
])
np.testing.assert_array_equal(O, p.O)
R = -np.broadcast_to(
np.array([
[1, 1, 1, 0],
[6, 6, 3, 1],
[3, 3, 3, 1],
[2, 2, 2, 1],
[3, 3, 3, 1],
])[:, :, None, None], (5, 4, 4, 4))
np.testing.assert_array_equal(R, p.R)
def setUp(self):
leaf = LeafCombination(
CollaborativeAction('Do it', (3., 2., 5.), human_probability=.3))
self.l2p = _NodeToPOMDP.from_node(leaf, 2., 1.)
def test_name_radix(self):
a = CollaborativeAction('My mixed Case action', (3., 2., 5.))
self.assertEqual(_name_radix(a), 'my-mixed-case-action')
C_INTR = 1.
C_ERR = 5.
INF = 100.
## Tested scenarios:
# 1. with full sequence of sequential actions
R_END = 0.1
LOOP = False
# 2. with full sequence of sequential actions
# R_END = 100
# LOOP = True
R_SUBTASK = None
## Define the task
mount_central = SequentialCombination([
LeafCombination(CollaborativeAction('Get central frame', (INF, 20., 30.))),
LeafCombination(
CollaborativeAction('Start Hold central frame', (3., 10., INF)))
],
name='Mount central frame')
#mount_legs = ParallelCombination([
mount_legs = SequentialCombination([
SequentialCombination([
LeafCombination(CollaborativeAction('Get left leg', (INF, 20., 30.))),
LeafCombination(
CollaborativeAction('Snap left leg', (5., INF, INF),
fail_probability=.1)),
],
name='Mount left leg'),
SequentialCombination([
LeafCombination(CollaborativeAction('Get right leg', (INF, 20., 30.))),