Esempi in Python per BDIStepTranslator

Esempio n. 1

File: test_bdi_step_translator.py Progetto: Gastd/oh-distro

 def test_stop_walking_no_queue(self):
     translator = BDIStepTranslator()
     def handle_steps(channel, msg_data):
         msg = atlas.behavior_step_params_t.decode(msg_data)
         self.assertEqual(msg.desired_step_spec.step_index, -1)
     lc = lcm.LCM()
     lc.subscribe('ATLAS_STEP_PARAMS', handle_steps)
     translator.handle_stop_walking('STOP_WALKING', None)
     lc.handle()

Esempio n. 2

File: test_bdi_step_translator.py Progetto: omelinopineda/Robot-Human

    def test_stop_walking_no_queue(self):
        translator = BDIStepTranslator()

        def handle_steps(channel, msg_data):
            msg = atlas.behavior_step_params_t.decode(msg_data)
            self.assertEqual(msg.desired_step_spec.step_index, -1)

        lc = lcm.LCM()
        lc.subscribe('ATLAS_STEP_PARAMS', handle_steps)
        translator.handle_stop_walking('STOP_WALKING', None)
        lc.handle()

Esempio n. 3

File: test_bdi_step_translator.py Progetto: Gastd/oh-distro

    def test_terrain_clearance(self):
        plan = self.generate_plan(drc.footstep_opts_t.BEHAVIOR_BDI_STEPPING)
        translator = BDIStepTranslator()

        def handle_steps(channel, msg_data):
            msg = atlas.behavior_step_params_t.decode(msg_data)
            self.assertAlmostEqual(msg.desired_step_spec.action.lift_height, plan.footsteps[2].params.bdi_lift_height + plan.footsteps[2].terrain_height[1])

        lc = lcm.LCM()
        lc.subscribe('ATLAS_STEP_PARAMS', handle_steps)
        translator.handle_footstep_plan('COMMITTED_FOOTSTEP_PLAN', plan.encode())
        lc.handle()

Esempio n. 4

File: test_bdi_step_translator.py Progetto: Gastd/oh-distro

    def test_stop_walking(self):
        plan = self.generate_plan(drc.footstep_opts_t.BEHAVIOR_BDI_STEPPING)
        translator = BDIStepTranslator()
        translator.handle_footstep_plan('COMMITTED_FOOTSTEP_PLAN', plan.encode())

        def handle_steps(channel, msg_data):
            msg = atlas.behavior_step_params_t.decode(msg_data)
            self.assertEqual(msg.desired_step_spec.step_index, -1)
        lc = lcm.LCM()
        lc.subscribe('ATLAS_STEP_PARAMS', handle_steps)
        translator.handle_stop_walking('STOP_WALKING', None)
        lc.handle()

Esempio n. 5

File: test_bdi_step_translator.py Progetto: omelinopineda/Robot-Human

    def test_terrain_clearance(self):
        plan = self.generate_plan(drc.footstep_opts_t.BEHAVIOR_BDI_STEPPING)
        translator = BDIStepTranslator()

        def handle_steps(channel, msg_data):
            msg = atlas.behavior_step_params_t.decode(msg_data)
            self.assertAlmostEqual(
                msg.desired_step_spec.action.lift_height,
                plan.footsteps[2].params.bdi_lift_height +
                plan.footsteps[2].terrain_height[1])

        lc = lcm.LCM()
        lc.subscribe('ATLAS_STEP_PARAMS', handle_steps)
        translator.handle_footstep_plan('COMMITTED_FOOTSTEP_PLAN',
                                        plan.encode())
        lc.handle()

Esempio n. 6

File: test_bdi_step_translator.py Progetto: Gastd/oh-distro

    def test_deprecated_walking(self):
        plan = self.generate_deprecated_plan(drc.footstep_opts_t.BEHAVIOR_BDI_WALKING)

        lc = lcm.LCM()
        lc.publish('CANDIDATE_FOOTSTEP_PLAN', plan.encode())

        translator = BDIStepTranslator()
        def handle_steps(channel, msg_data):
            msg = atlas.behavior_walk_params_t.decode(msg_data)
            self.assertAlmostEqual(msg.walk_spec_queue[0].foot.position[0], 0.04)
            self.assertAlmostEqual(msg.walk_spec_queue[0].foot.position[1], 0)
            self.assertAlmostEqual(msg.walk_spec_queue[0].foot.position[2], 0)
            self.assertAlmostEqual(msg.walk_spec_queue[0].foot.normal[0], 0)
            self.assertAlmostEqual(msg.walk_spec_queue[0].foot.normal[1], 0)
            self.assertAlmostEqual(msg.walk_spec_queue[0].foot.normal[2], 1)
            self.assertEqual(msg.use_spec, True)
        lc.subscribe('ATLAS_WALK_PARAMS', handle_steps)
        translator.handle_footstep_plan('COMMITTED_FOOTSTEP_PLAN', plan.encode())
        lc.handle()

        plotter = BDIStepTranslator(mode=Mode.plotting)
        plotter.handle_footstep_plan('CANDIDATE_FOOTSTEP_PLAN', plan.encode())
        self.assertAlmostEqual(translator.bdi_step_queue_out[0].foot.position[0], 0.04)
        self.assertAlmostEqual(translator.bdi_step_queue_out[0].foot.position[1], 0)
        self.assertAlmostEqual(translator.bdi_step_queue_out[0].foot.position[2], 0)
        self.assertAlmostEqual(translator.bdi_step_queue_out[0].foot.normal[0], 0)
        self.assertAlmostEqual(translator.bdi_step_queue_out[0].foot.normal[1], 0)
        self.assertAlmostEqual(translator.bdi_step_queue_out[0].foot.normal[2], 1)
        self.assertEqual(translator.use_spec, True)

Esempio n. 7

File: test_bdi_step_translator.py Progetto: tkoolen/oh-distro

    def test_stepping(self):
        plan = self.generate_plan(drc.footstep_opts_t.BEHAVIOR_BDI_STEPPING)
        lc = lcm.LCM()
        lc.publish('CANDIDATE_FOOTSTEP_PLAN', plan.encode())

        translator = BDIStepTranslator()
        def handle_steps(channel, msg_data):
            msg = drc.atlas_behavior_step_params_t.decode(msg_data)
            self.assertAlmostEqual(msg.desired_step_spec.foot.position[0], 0.04)
            self.assertAlmostEqual(msg.desired_step_spec.foot.position[1], 0)
            self.assertAlmostEqual(msg.desired_step_spec.foot.position[2], 0)
            self.assertAlmostEqual(msg.desired_step_spec.foot.normal[0], 0)
            self.assertAlmostEqual(msg.desired_step_spec.foot.normal[1], 0)
            self.assertAlmostEqual(msg.desired_step_spec.foot.normal[2], 1)
            self.assertEqual(msg.use_spec, True)
        lc.subscribe('ATLAS_STEP_PARAMS', handle_steps)
        translator.handle_footstep_plan('COMMITTED_FOOTSTEP_PLAN', plan.encode())
        lc.handle()
        # import pdb; pdb.set_trace()

        self.assertAlmostEqual(translator.bdi_step_queue_out[0].foot.position[0], 0.04)
        self.assertAlmostEqual(translator.bdi_step_queue_out[0].foot.position[1], 0)
        self.assertAlmostEqual(translator.bdi_step_queue_out[0].foot.position[2], 0)
        self.assertAlmostEqual(translator.bdi_step_queue_out[0].foot.normal[0], 0)
        self.assertAlmostEqual(translator.bdi_step_queue_out[0].foot.normal[1], 0)
        self.assertAlmostEqual(translator.bdi_step_queue_out[0].foot.normal[2], 1)
        self.assertEqual(translator.use_spec, True)

        plotter = BDIStepTranslator(mode=Mode.plotting)
        plotter.handle_footstep_plan('CANDIDATE_FOOTSTEP_PLAN', plan.encode())

Esempio n. 8

File: test_bdi_step_translator.py Progetto: omelinopineda/Robot-Human

    def test_atlas_status(self):
        plan = self.generate_plan(drc.footstep_opts_t.BEHAVIOR_BDI_STEPPING)
        lc = lcm.LCM()
        lc.publish('CANDIDATE_FOOTSTEP_PLAN', plan.encode())

        translator = BDIStepTranslator()
        translator.handle_footstep_plan('COMMITTED_FOOTSTEP_PLAN',
                                        plan.encode())
        self.assertEqual(translator.delivered_index, 1)

        status = atlas.status_t()
        status.step_feedback = atlas.step_feedback_t()
        status.step_feedback.next_step_index_needed = 2
        translator.executing = True
        translator.handle_atlas_status('ATLAS_STATUS', status)
        translator.executing = False
        self.assertEqual(translator.delivered_index, 2)

Esempio n. 9

File: test_bdi_step_translator.py Progetto: Gastd/oh-distro

    def test_atlas_status(self):
        plan = self.generate_plan(drc.footstep_opts_t.BEHAVIOR_BDI_STEPPING)
        lc = lcm.LCM()
        lc.publish('CANDIDATE_FOOTSTEP_PLAN', plan.encode())

        translator = BDIStepTranslator()
        translator.handle_footstep_plan('COMMITTED_FOOTSTEP_PLAN', plan.encode())
        self.assertEqual(translator.delivered_index, 1)

        status = atlas.status_t()
        status.step_feedback = atlas.step_feedback_t()
        status.step_feedback.next_step_index_needed = 2
        translator.executing = True
        translator.handle_atlas_status('ATLAS_STATUS', status)
        translator.executing = False
        self.assertEqual(translator.delivered_index, 2)

Esempio n. 10

File: test_bdi_step_translator.py Progetto: omelinopineda/Robot-Human

    def test_deprecated_stepping(self):
        plan = self.generate_deprecated_plan(
            drc.footstep_opts_t.BEHAVIOR_BDI_STEPPING)
        lc = lcm.LCM()
        lc.publish('CANDIDATE_FOOTSTEP_PLAN', plan.encode())

        translator = BDIStepTranslator()

        def handle_steps(channel, msg_data):
            msg = atlas.behavior_step_params_t.decode(msg_data)
            self.assertAlmostEqual(msg.desired_step_spec.foot.position[0],
                                   0.04)
            self.assertAlmostEqual(msg.desired_step_spec.foot.position[1], 0)
            self.assertAlmostEqual(msg.desired_step_spec.foot.position[2], 0)
            self.assertAlmostEqual(msg.desired_step_spec.foot.normal[0], 0)
            self.assertAlmostEqual(msg.desired_step_spec.foot.normal[1], 0)
            self.assertAlmostEqual(msg.desired_step_spec.foot.normal[2], 1)
            self.assertEqual(msg.use_spec, True)

        lc.subscribe('ATLAS_STEP_PARAMS', handle_steps)
        translator.handle_footstep_plan('COMMITTED_FOOTSTEP_PLAN',
                                        plan.encode())
        lc.handle()
        # import pdb; pdb.set_trace()

        self.assertAlmostEqual(
            translator.bdi_step_queue_out[0].foot.position[0], 0.04)
        self.assertAlmostEqual(
            translator.bdi_step_queue_out[0].foot.position[1], 0)
        self.assertAlmostEqual(
            translator.bdi_step_queue_out[0].foot.position[2], 0)
        self.assertAlmostEqual(translator.bdi_step_queue_out[0].foot.normal[0],
                               0)
        self.assertAlmostEqual(translator.bdi_step_queue_out[0].foot.normal[1],
                               0)
        self.assertAlmostEqual(translator.bdi_step_queue_out[0].foot.normal[2],
                               1)
        self.assertEqual(translator.use_spec, True)

        plotter = BDIStepTranslator(mode=Mode.plotting)
        plotter.handle_footstep_plan('CANDIDATE_FOOTSTEP_PLAN', plan.encode())

Esempio n. 11

File: test_bdi_step_translator.py Progetto: omelinopineda/Robot-Human

    def test_stop_walking(self):
        plan = self.generate_plan(drc.footstep_opts_t.BEHAVIOR_BDI_STEPPING)
        translator = BDIStepTranslator()
        translator.handle_footstep_plan('COMMITTED_FOOTSTEP_PLAN',
                                        plan.encode())

        def handle_steps(channel, msg_data):
            msg = atlas.behavior_step_params_t.decode(msg_data)
            self.assertEqual(msg.desired_step_spec.step_index, -1)

        lc = lcm.LCM()
        lc.subscribe('ATLAS_STEP_PARAMS', handle_steps)
        translator.handle_stop_walking('STOP_WALKING', None)
        lc.handle()

Esempio n. 12

File: run_plotter.py Progetto: omelinopineda/Robot-Human

def run_plotter():
    t = BDIStepTranslator(mode=Mode.plotting)
    t.run()

Esempio n. 13

File: run_plotter.py Progetto: Gastd/oh-distro

def run_plotter():
    t = BDIStepTranslator(mode=Mode.plotting)
    t.run()

Esempio n. 14

def run_plotter():
    t = BDIStepTranslator(mode=Mode.plotting, safe=False)
    t.run()

Esempio n. 15

def run_translator():
    t = BDIStepTranslator(mode=Mode.translating, safe=False)
    t.run()