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.))),