diff --git a/examples/interfaces/playstation.py b/examples/interfaces/playstation.py index fb7db5c..78a96f6 100644 --- a/examples/interfaces/playstation.py +++ b/examples/interfaces/playstation.py @@ -8,16 +8,20 @@ $ python playstation.py --controller ps # to use any PS game controller $ python playstation.py --controller ps3 # to use PS3 game controller $ python playstation.py --controller ps4 # to use PS4 game controller ``` + +Note that these game controllers are blocking by default, thus set `use_thread` to True to avoid to be blocked. """ import time -from itertools import count import argparse from pyrobolearn.tools.interfaces.controllers.playstation import * + # create parser to select the game controller parser = argparse.ArgumentParser() +parser.add_argument('-t', '--use_thread', help='If we should run the PlayStation controller in a thread.', type=bool, + default=True) parser.add_argument('-c', '--controller', help='The Playstation game controller to use (ps, ps3, or ps4)', type=str, choices=['ps', 'ps3', 'ps4'], default='ps') args = parser.parse_args() @@ -25,25 +29,25 @@ args = parser.parse_args() # load corresponding Playstation controller interface if args.controller == 'ps': - controller = PSControllerInterface(verbose=False) + controller = PSControllerInterface(use_thread=args.use_thread, verbose=False) if args.controller == 'ps3': - controller = PS3ControllerInterface(verbose=False) + controller = PS3ControllerInterface(use_thread=args.use_thread, verbose=False) elif args.controller == 'ps4': - controller = PS4ControllerInterface(verbose=False) + controller = PS4ControllerInterface(use_thread=args.use_thread, verbose=False) else: raise NotImplementedError("Unknown game controller") # run controller print('Running controller...') -for _ in count(): +while True: # run one step with the interface - controller.run() # same as `step()` if we are not using threads + controller.step() # same as `step()` if we are not using threads # get the last update and print it - b = controller.X - print("X: {}".format(b)) # , controller[b])) + b = controller.last_updated_button + print("Last updated button: {} with value: {}".format(b, controller[b])) # sleep a bit time.sleep(0.01) diff --git a/examples/interfaces/robots/quadcopter_controller.py b/examples/interfaces/robots/quadcopter_controller.py new file mode 100644 index 0000000..2274649 --- /dev/null +++ b/examples/interfaces/robots/quadcopter_controller.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python +"""Control a quadcopter in the air using an Xbox or Playstation game controller. + +how to run: +``` +$ python quadcopter_controller.py --help # for help +$ python quadcopter_controller.py --controller keyboard # to use the keyboard +$ python quadcopter_controller.py --controller xbox # to use Xbox game controller +$ python quadcopter_controller.py --controller ps # to use PS game controller +``` + +Mapping of the keyboard interface: +- `top arrow`: move forward +- `bottom arrow`: move backward +- `left arrow`: move sideways to the left +- `right arrow`: move sideways to the right +- `ctrl + top arrow`: ascend +- `ctrl + bottom arrow`: descend +- `ctrl + left arrow`: turn to the right +- `ctrl + right arrow`: turn to the left +- `space`: switch between first-person and third-person view + +Mapping between the controller and the quadcopter: +- left joystick: use to move the quadcopter +- right joystick: use to ascend/descend and turn +- south button (X on PlayStation and A on Xbox): change between the first-person and third-person view. +- east button (circle on PlayStation and B on Xbox): increase the speed +- west button (square on PlayStation and X on Xbox): decrease the speed +""" + +# import numpy as np +from itertools import count +import argparse + +import pyrobolearn as prl + + +# create parser to select the game controller +parser = argparse.ArgumentParser() +parser.add_argument('-c', '--controller', help='the controller to use', type=str, + choices=['keyboard', 'xbox', 'ps'], default='keyboard') +args = parser.parse_args() + + +# load corresponding interface +if args.controller == 'keyboard': # keyboard interface + from pyrobolearn.tools.interfaces.mouse_keyboard.mousekeyboard import MouseKeyboardInterface as Controller + from pyrobolearn.tools.bridges.mouse_keyboard.bridge_mousekeyboard_quadcopter \ + import BridgeMouseKeyboardQuadcopter as Bridge +elif args.controller == 'xbox': # Xbox interface + from pyrobolearn.tools.interfaces.controllers.xbox import XboxControllerInterface as Controller + from pyrobolearn.tools.bridges.controllers.robots.bridge_controller_quadcopter import BridgeControllerQuadcopter \ + as Bridge +elif args.controller == 'ps': # PS interface + from pyrobolearn.tools.interfaces.controllers.playstation import PSControllerInterface as Controller + from pyrobolearn.tools.bridges.controllers.robots.bridge_controller_quadcopter import BridgeControllerQuadcopter \ + as Bridge +else: + raise NotImplementedError("Unknown game controller") + + +# create simulator +sim = prl.simulators.Bullet() + +# create basic world (with a floor and gravity enabled by default) +world = prl.worlds.BasicWorld(sim) + +# load quadcopter +robot = prl.robots.Quadcopter(sim, position=[0., 0., 2.]) +world.load_robot(robot) + +# load interface that accepts input events +controller = Controller(use_thread=True, verbose=False) + +# load bridge that connects the interface/controller with the quadcopter +# The bridge is the one that maps the input events from the interface to commands sent to the quadcopter +bridge = Bridge(quadcopter=robot, interface=controller) + +# run simulator +for t in count(): + # perform a step with the bridge and interface + bridge.step(update_interface=True) + + # perform one step in the world + world.step(sleep_dt=1. / 240) diff --git a/examples/interfaces/robots/quadcopter_speech.py b/examples/interfaces/robots/quadcopter_speech.py new file mode 100644 index 0000000..c77dae6 --- /dev/null +++ b/examples/interfaces/robots/quadcopter_speech.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python +"""Control a quadcopter in the air using speech. + +Try to say: +- turn right/left +- move/go higher/lower/forward/backward/right/left +- go faster/slower +- anything else will ask the robot to hover +""" + +from itertools import count + +import pyrobolearn as prl +from pyrobolearn.tools.bridges.audio.robots.bridge_speech_quadcopter import BridgeSpeechRecognizerQuadcopter + + +# create simulator +sim = prl.simulators.Bullet() + +# create basic world (with a floor and gravity enabled by default) +world = prl.worlds.BasicWorld(sim) + +# load quadcopter +robot = prl.robots.Quadcopter(sim, position=[0., 0., 2.]) +world.load_robot(robot) + +# load bridge that connects the speech interface with the quadcopter +# Note that it will create automatically the Speech Recognizer interface inside the bridge +bridge = BridgeSpeechRecognizerQuadcopter(robot=robot, interface=None, verbose=True) + + +# run simulator +for t in count(): + # perform a step with the bridge and interface + bridge.step(update_interface=True) + + # ask the world camera to follow the wheeled robot + world.follow(robot, distance=2) + + # perform one step in the world + world.step(sleep_dt=1. / 240) diff --git a/examples/interfaces/robots/wheeled_controller.py b/examples/interfaces/robots/wheeled_controller.py new file mode 100644 index 0000000..2ec9f2e --- /dev/null +++ b/examples/interfaces/robots/wheeled_controller.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python +"""Control a wheeled robot using a keyboard, an Xbox or Playstation game controller. + +how to run: +``` +$ python wheeled_controller.py --help # for help +$ python wheeled_controller.py --controller keyboard # to use the keyboard +$ python wheeled_controller.py --controller xbox # to use Xbox game controller +$ python wheeled_controller.py --controller ps # to use PS game controller +``` + +Mapping of the keyboard interface: +* `top arrow`: move forward +* `bottom arrow`: move backward +* `left arrow`: turn/steer to the left +* `right arrow`: turn/steer to the right + +Mapping between the controller and the wheeled robot: +- left joystick: velocity of the wheeled robot +- east button (circle on PlayStation and B on Xbox): increase the speed +- west button (square on PlayStation and X on Xbox): decrease the speed +""" + +# import numpy as np +from itertools import count +import argparse + +import pyrobolearn as prl + + +# create parser to select the game controller +parser = argparse.ArgumentParser() +parser.add_argument('-c', '--controller', help='the controller to use', type=str, + choices=['keyboard', 'xbox', 'ps'], default='keyboard') +args = parser.parse_args() + + +# load corresponding interface +if args.controller == 'keyboard': # keyboard interface + from pyrobolearn.tools.interfaces.mouse_keyboard.mousekeyboard import MouseKeyboardInterface as Controller + from pyrobolearn.tools.bridges.mouse_keyboard.bridge_mousekeyboard_wheeled \ + import BridgeMouseKeyboardDifferentialWheeledRobot as Bridge +elif args.controller == 'xbox': # Xbox interface + from pyrobolearn.tools.interfaces.controllers.xbox import XboxControllerInterface as Controller + from pyrobolearn.tools.bridges.controllers.robots.bridge_controller_wheeled import BridgeControllerWheeledRobot \ + as Bridge +elif args.controller == 'ps': # PS interface + from pyrobolearn.tools.interfaces.controllers.playstation import PSControllerInterface as Controller + from pyrobolearn.tools.bridges.controllers.robots.bridge_controller_wheeled import \ + BridgeControllerWheeledRobot as Bridge +else: + raise NotImplementedError("Unknown game controller") + + +# create simulator +sim = prl.simulators.Bullet() + +# create basic world (with a floor and gravity enabled by default) +world = prl.worlds.BasicWorld(sim) + +# load quadcopter +robot = prl.robots.Epuck(sim, position=[0., 0.]) +world.load_robot(robot) + +# load interface that accepts input events +controller = Controller(use_thread=True, verbose=False) + +# load bridge that connects the interface/controller with the quadcopter +# The bridge is the one that maps the input events from the interface to commands sent to the quadcopter +bridge = Bridge(robot=robot, interface=controller) + + +# run simulator +for t in count(): + # perform a step with the bridge and interface + bridge.step(update_interface=True) + + # perform one step in the world + world.step(sleep_dt=1. / 240) diff --git a/examples/interfaces/robots/wheeled_speech.py b/examples/interfaces/robots/wheeled_speech.py new file mode 100644 index 0000000..b320eff --- /dev/null +++ b/examples/interfaces/robots/wheeled_speech.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python +"""Control a wheeled robot in the air using speech. + +Try to say: +- turn right/left +- move/go forward/backward +- go faster/slower +- anything else will ask the robot to hover +""" + +from itertools import count + +import pyrobolearn as prl +from pyrobolearn.tools.bridges.audio.robots.bridge_speech_wheeled import BridgeSpeechRecognizerDifferentialWheeledRobot + + +# create simulator +sim = prl.simulators.Bullet() + +# create basic world (with a floor and gravity enabled by default) +world = prl.worlds.BasicWorld(sim) + +# load wheeled robot +robot = prl.robots.Epuck(sim, position=[0., 0.]) +world.load_robot(robot) + +# load bridge that connects the speech interface with the wheeled robot +# Note that it will create automatically the Speech Recognizer interface inside the bridge +bridge = BridgeSpeechRecognizerDifferentialWheeledRobot(robot=robot, interface=None, verbose=True) + + +# run simulator +for t in count(): + # perform a step with the bridge and interface + bridge.step(update_interface=True) + + # ask the world camera to follow the wheeled robot + world.follow(robot, distance=2) + + # perform one step in the world + world.step(sleep_dt=1. / 240) diff --git a/examples/interfaces/speech.py b/examples/interfaces/speech.py index 51b51d4..a47ec2a 100644 --- a/examples/interfaces/speech.py +++ b/examples/interfaces/speech.py @@ -33,4 +33,4 @@ interface = SpeechTranslatorInterface(verbose=True, from_lang=args.lang, target_ # run the interface while True: - data = interface.run() + data = interface.step() diff --git a/examples/interfaces/webcam.py b/examples/interfaces/webcam.py index 3ad2c6d..e839fe0 100644 --- a/examples/interfaces/webcam.py +++ b/examples/interfaces/webcam.py @@ -33,7 +33,7 @@ else: plt.ion() # interactive mode on while True: - # if don't use thread call `step` or `run` (note that `run` returns the frame but not + # perform a `step` with the interface interface.step() # get the frame and plot it with matplotlib diff --git a/examples/interfaces/xbox.py b/examples/interfaces/xbox.py index f32d3a5..03a8ca8 100644 --- a/examples/interfaces/xbox.py +++ b/examples/interfaces/xbox.py @@ -1,35 +1,50 @@ #!/usr/bin/env python """Load the Xbox game controller interface + +How to run: +``` +$ python xbox.py --help # for help +$ python xbox.py --controller xbox # to use any Xbox game controller (by default) +$ python xbox.py --controller xbox-360 # to use Xbox 360 game controller +$ python xbox.py --controller xbox-one # to use Xbox One game controller +``` + +Note that these game controllers are blocking by default, thus set `use_thread` to True to avoid to be blocked. """ import time -from itertools import count import argparse -from pyrobolearn.tools.interfaces.controllers.xbox import Xbox360ControllerInterface, XboxOneControllerInterface +from pyrobolearn.tools.interfaces.controllers.xbox import XboxControllerInterface, XboxOneControllerInterface, \ + Xbox360ControllerInterface + # create parser to select the game controller parser = argparse.ArgumentParser() -parser.add_argument('-c', '--controller', help='The Xbox game controller to use (xbox one or xbox 360)', type=str, - choices=['360', 'one'], default='one') +parser.add_argument('-t', '--use_thread', help='If we should run the Xbox controller in a thread.', type=bool, + default=True) +parser.add_argument('-c', '--controller', help='The Xbox game controller to use.', type=str, + choices=['xbox', 'xbox-360', 'xbox-one'], default='xbox') args = parser.parse_args() # load corresponding Xbox controller interface -if args.controller == '360': - controller = Xbox360ControllerInterface(verbose=True) -elif args.controller == 'one': - controller = XboxOneControllerInterface(verbose=True) +if args.controller == 'xbox': + controller = XboxControllerInterface(use_thread=args.use_thread, verbose=False) +elif args.controller == 'xbox-360': + controller = Xbox360ControllerInterface(use_thread=args.use_thread, verbose=False) +elif args.controller == 'xbox-one': + controller = XboxOneControllerInterface(use_thread=args.use_thread, verbose=False) else: raise NotImplementedError("Unknown game controller") # run controller print('Running controller...') -for _ in count(): +while True: # run one step with the interface - controller.run() # same as `step()` if we are not using threads + controller.step() # get the last update and print it b = controller.last_updated_button diff --git a/examples/robots/quadcopter_controller.py b/examples/robots/quadcopter_controller.py index c1dde02..2274649 100644 --- a/examples/robots/quadcopter_controller.py +++ b/examples/robots/quadcopter_controller.py @@ -19,6 +19,13 @@ Mapping of the keyboard interface: - `ctrl + left arrow`: turn to the right - `ctrl + right arrow`: turn to the left - `space`: switch between first-person and third-person view + +Mapping between the controller and the quadcopter: +- left joystick: use to move the quadcopter +- right joystick: use to ascend/descend and turn +- south button (X on PlayStation and A on Xbox): change between the first-person and third-person view. +- east button (circle on PlayStation and B on Xbox): increase the speed +- west button (square on PlayStation and X on Xbox): decrease the speed """ # import numpy as np @@ -63,7 +70,7 @@ robot = prl.robots.Quadcopter(sim, position=[0., 0., 2.]) world.load_robot(robot) # load interface that accepts input events -controller = Controller() +controller = Controller(use_thread=True, verbose=False) # load bridge that connects the interface/controller with the quadcopter # The bridge is the one that maps the input events from the interface to commands sent to the quadcopter diff --git a/examples/robots/visualize_robot.py b/examples/robots/visualize_robot.py index b63138f..2f93b6c 100644 --- a/examples/robots/visualize_robot.py +++ b/examples/robots/visualize_robot.py @@ -36,8 +36,7 @@ import pyrobolearn as prl # create parser to select the robot robots = ['coman', 'hyq2max'] # prl.robots.implemented_robots parser = argparse.ArgumentParser() -parser.add_argument('-r', '--robot', help='the robot to load in the world', type=str, - choices=robots, default='hyq2max') +parser.add_argument('-r', '--robot', help='the robot to load in the world', type=str, choices=robots, default='hyq2max') args = parser.parse_args() @@ -57,7 +56,8 @@ for _ in range(100): # change visualization robot.change_transparency() robot.draw_link_coms() -robot.draw_link_frames() +robot.draw_link_frames(robot.legs[0]) +robot.draw_joint_frames(robot.legs[0]) robot.draw_bounding_boxes(link_ids=-1) robot.draw_friction_cone(floor_id=world.floor_id) diff --git a/examples/robots/wheeled_controller.py b/examples/robots/wheeled_controller.py new file mode 100644 index 0000000..2ec9f2e --- /dev/null +++ b/examples/robots/wheeled_controller.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python +"""Control a wheeled robot using a keyboard, an Xbox or Playstation game controller. + +how to run: +``` +$ python wheeled_controller.py --help # for help +$ python wheeled_controller.py --controller keyboard # to use the keyboard +$ python wheeled_controller.py --controller xbox # to use Xbox game controller +$ python wheeled_controller.py --controller ps # to use PS game controller +``` + +Mapping of the keyboard interface: +* `top arrow`: move forward +* `bottom arrow`: move backward +* `left arrow`: turn/steer to the left +* `right arrow`: turn/steer to the right + +Mapping between the controller and the wheeled robot: +- left joystick: velocity of the wheeled robot +- east button (circle on PlayStation and B on Xbox): increase the speed +- west button (square on PlayStation and X on Xbox): decrease the speed +""" + +# import numpy as np +from itertools import count +import argparse + +import pyrobolearn as prl + + +# create parser to select the game controller +parser = argparse.ArgumentParser() +parser.add_argument('-c', '--controller', help='the controller to use', type=str, + choices=['keyboard', 'xbox', 'ps'], default='keyboard') +args = parser.parse_args() + + +# load corresponding interface +if args.controller == 'keyboard': # keyboard interface + from pyrobolearn.tools.interfaces.mouse_keyboard.mousekeyboard import MouseKeyboardInterface as Controller + from pyrobolearn.tools.bridges.mouse_keyboard.bridge_mousekeyboard_wheeled \ + import BridgeMouseKeyboardDifferentialWheeledRobot as Bridge +elif args.controller == 'xbox': # Xbox interface + from pyrobolearn.tools.interfaces.controllers.xbox import XboxControllerInterface as Controller + from pyrobolearn.tools.bridges.controllers.robots.bridge_controller_wheeled import BridgeControllerWheeledRobot \ + as Bridge +elif args.controller == 'ps': # PS interface + from pyrobolearn.tools.interfaces.controllers.playstation import PSControllerInterface as Controller + from pyrobolearn.tools.bridges.controllers.robots.bridge_controller_wheeled import \ + BridgeControllerWheeledRobot as Bridge +else: + raise NotImplementedError("Unknown game controller") + + +# create simulator +sim = prl.simulators.Bullet() + +# create basic world (with a floor and gravity enabled by default) +world = prl.worlds.BasicWorld(sim) + +# load quadcopter +robot = prl.robots.Epuck(sim, position=[0., 0.]) +world.load_robot(robot) + +# load interface that accepts input events +controller = Controller(use_thread=True, verbose=False) + +# load bridge that connects the interface/controller with the quadcopter +# The bridge is the one that maps the input events from the interface to commands sent to the quadcopter +bridge = Bridge(robot=robot, interface=controller) + + +# run simulator +for t in count(): + # perform a step with the bridge and interface + bridge.step(update_interface=True) + + # perform one step in the world + world.step(sleep_dt=1. / 240) diff --git a/pyrobolearn/tools/bridges/audio/robots/bridge_speech_quadcopter.py b/pyrobolearn/tools/bridges/audio/robots/bridge_speech_quadcopter.py new file mode 100644 index 0000000..81e96d1 --- /dev/null +++ b/pyrobolearn/tools/bridges/audio/robots/bridge_speech_quadcopter.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python +"""Bridges between speech interface and quadcopter +""" + +from pyrobolearn.robots import Quadcopter +from pyrobolearn.tools.interfaces.audio.speech import SpeechRecognizerInterface +from pyrobolearn.tools.bridges.bridge import Bridge + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class BridgeSpeechRecognizerQuadcopter(Bridge): + r"""Bridge Speech Wheeled Robot + + Bridge between the speech recognizer interface and a wheeled robot. You can give oral orders to the robot. + """ + + def __init__(self, robot, interface=None, speed=10., priority=None, verbose=False, *args, **kwargs): + """ + Initialize the bridge between the speech recognizer interface and the quadcopter robot. + + Args: + robot (Quadcopter): quadcopter robot instance. + interface (SpeechRecognizerInterface, None): speech recognizer interface. If None, it will instantiate it + here, and launch it in a thread. + speed (float): initial speed of the quadcopter robot. + priority (int): priority of the bridge. + verbose (bool): If True, print information on the standard output. + """ + # check the robot + if not isinstance(quadcopter, Quadcopter): + raise TypeError("Expecting the given 'quadcopter' to be an instance of `Quadcopter`, but got instead: " + "{}".format(type(robot))) + self.robot = robot + self.speed = speed + + if interface is None: + interface = SpeechRecognizerInterface(use_thread=True, verbose=verbose) + if not isinstance(interface, SpeechRecognizerInterface): + raise TypeError("Expecting the given 'interface' to be an instance of `SpeechRecognizerInterface`, but " + "got instead: {}".format(type(interface))) + + super(BridgeSpeechRecognizerQuadcopter, self).__init__(interface, priority=priority, verbose=verbose) + + def step(self, update_interface=False): + """Perform a step with the bridge.""" + # update interface + if update_interface: + self.interface() + + # get the data + data = self.interface.data + + if self.verbose and data is not None: + print("Bridge: the data = {}".format(data)) + + # split the data + data = data.split() + + if data[-1] == 'higher': + self.robot.ascend(speed=self.speed) + elif data[-1] == 'lower': + self.robot.descend(speed=self.speed) + elif data[-1] == 'forward': + self.robot.move_forward(speed=self.speed) + elif data[-1] == 'backward': + self.robot.move_backward(speed=self.speed) + elif data == 'turn right': + self.robot.turn_right(speed=self.speed) + elif data == 'turn left': + self.robot.turn_left(speed=self.speed) + elif data[-1] == 'right': + self.robot.move_right(speed=self.speed) + elif data[-1] == 'left': + self.robot.move_left(speed=self.speed) + elif data[-1] == 'faster': + self.speed *= 2. + elif data[-1] == 'slower': + self.speed /= 2. + else: + self.robot.hover() diff --git a/pyrobolearn/tools/bridges/audio/robots/bridge_speech_rotatory_uav.py b/pyrobolearn/tools/bridges/audio/robots/bridge_speech_rotatory_uav.py deleted file mode 100644 index 1f23788..0000000 --- a/pyrobolearn/tools/bridges/audio/robots/bridge_speech_rotatory_uav.py +++ /dev/null @@ -1,63 +0,0 @@ -#!/usr/bin/env python -"""Bridges between audio interface and rotatory wing robots -""" - -from pyrobolearn.robots import RotaryWingUAV -from pyrobolearn.tools.interfaces.audio.audio import SpeechRecognizerInterface -from pyrobolearn.tools.bridges.bridge import Bridge - - -__author__ = "Brian Delhaisse" -__copyright__ = "Copyright 2018, PyRoboLearn" -__credits__ = ["Brian Delhaisse"] -__license__ = "GNU GPLv3" -__version__ = "1.0.0" -__maintainer__ = "Brian Delhaisse" -__email__ = "briandelhaisse@gmail.com" -__status__ = "Development" - - -class BridgeSpeechRecognizerRotatoryUAV(Bridge): - r"""Bridge Speech Wheeled Robot - - Bridge between the speech recognizer interface and a wheeled robot. You can give oral orders to the robot. - """ - - def __init__(self, interface, uav_robot, init_speed=1.): - if not isinstance(interface, SpeechRecognizerInterface): - raise TypeError("Expecting a speech recognizer interface") - if not isinstance(uav_robot, RotaryWingUAV): - raise TypeError("Expecting a wheeled robot") - super(BridgeSpeechRecognizerRotatoryUAV, self).__init__(interface) - self.robot = uav_robot - self.speed = init_speed - - def step(self): - data = self.interface.data - data = data.split() - # print('data: {}'.format(data)) - if data[0] == 'stop' or data[0] == 'stay': - self.robot.stop() - elif data[-1] == 'higher': - pass - elif data[-1] == 'lower': - pass - elif data[-1] == 'forward': - pass - elif data[-1] == 'backward': - pass - elif data == 'turn right': - pass - elif data == 'turn left': - pass - elif data[-1] == 'right': - pass - elif data[-1] == 'left': - pass - elif data[-1] == 'faster': - self.speed *= 2. - elif data[-1] == 'slower': - self.speed /= 2. - elif data: - pass - # print('I do not know the meaning of {}'.format(data)) diff --git a/pyrobolearn/tools/bridges/audio/robots/bridge_speech_wheeled.py b/pyrobolearn/tools/bridges/audio/robots/bridge_speech_wheeled.py index 717dca9..0b1288d 100644 --- a/pyrobolearn/tools/bridges/audio/robots/bridge_speech_wheeled.py +++ b/pyrobolearn/tools/bridges/audio/robots/bridge_speech_wheeled.py @@ -1,13 +1,14 @@ #!/usr/bin/env python -"""Bridges between audio interface and wheeled robots +"""Bridges between speech interface and wheeled robots """ import numpy as np -from pyrobolearn.robots import WheeledRobot, AckermannWheeledRobot -from pyrobolearn.tools.interfaces.audio.audio import SpeechRecognizerInterface +from pyrobolearn.robots import WheeledRobot, DifferentialWheeledRobot, AckermannWheeledRobot +from pyrobolearn.tools.interfaces.audio.speech import SpeechRecognizerInterface from pyrobolearn.tools.bridges.bridge import Bridge + __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] @@ -24,71 +25,137 @@ class BridgeSpeechRecognizerWheeledRobot(Bridge): Bridge between the speech recognizer interface and a wheeled robot. You can give oral orders to the robot. """ - def __init__(self, interface, wheeled_robot, init_speed=1.): - if not isinstance(interface, SpeechRecognizerInterface): - raise TypeError("Expecting a speech recognizer interface") - if not isinstance(wheeled_robot, WheeledRobot): - raise TypeError("Expecting a wheeled robot") - super(BridgeSpeechRecognizerWheeledRobot, self).__init__(interface) - self.robot = wheeled_robot - self.speed = init_speed + def __init__(self, robot, interface=None, speed=1., priority=None, verbose=False): + """ + Initialize the bridge between the speech recognizer interface and the wheeled robot. - def step(self): + Args: + robot (WheeledRobot): wheeled robot instance. + interface (SpeechRecognizerInterface, None): speech recognizer interface. If None, it will instantiate it + here, and launch it in a thread. + speed (float): initial speed of the wheeled robot. + priority (int): priority of the bridge. + verbose (bool): If True, print information on the standard output. + """ + # check the robot + if not isinstance(robot, WheeledRobot): + raise TypeError("Expecting a wheeled robot, instead got: {}".format(robot)) + self.robot = robot + self.speed = speed + + # check the interface + if interface is None: + interface = SpeechRecognizerInterface(use_thread=True, verbose=verbose) + if not isinstance(interface, SpeechRecognizerInterface): + raise TypeError("Expecting a speech recognizer interface, instead got: {}".format(type(interface))) + + super(BridgeSpeechRecognizerWheeledRobot, self).__init__(interface, priority=priority, verbose=verbose) + + def step(self, update_interface=False): + """Perform a step with the bridge.""" + # update interface + if update_interface: + self.interface() + + +class BridgeSpeechRecognizerDifferentialWheeledRobot(BridgeSpeechRecognizerWheeledRobot): + r"""Bridge between Speech Recognizer and Differential Wheeled Robot + + Bridge between the speech recognizer interface and a differential wheeled robot. You can give oral + orders to the robot. + """ + + def __init__(self, robot, interface=None, speed=1., priority=None, verbose=False): + """ + Initialize the bridge between the speech recognizer interface and the wheeled robot. + + Args: + robot (DifferentialWheeledRobot): wheeled robot instance. + interface (SpeechRecognizerInterface, None): speech recognizer interface. If None, it will instantiate it + here, and launch it in a thread. + speed (float): initial speed of the wheeled robot. + verbose (bool): If True, print information on the standard output. + """ + if not isinstance(robot, DifferentialWheeledRobot): + raise TypeError("Expecting a wheeled robot of type Ackermann steering, instead got: {}".format(robot)) + + super(BridgeSpeechRecognizerDifferentialWheeledRobot, self).__init__(robot=robot, interface=interface, + speed=speed, priority=priority, + verbose=verbose) + + def step(self, update_interface=False): + """Perform a step with the bridge.""" + super(BridgeSpeechRecognizerDifferentialWheeledRobot, self).step(update_interface=update_interface) + + # get the data from the interface data = self.interface.data - #print('data: {}'.format(data)) - if data == 'stop': - self.robot.stop() - elif data == 'move forward': + + if self.verbose and data is not None: + print("Bridge: the data = {}".format(data)) + + if data == 'move forward': self.robot.drive_forward(self.speed) elif data == 'move backward': self.robot.drive_backward(self.speed) elif data == 'turn right': - pass + self.robot.turn_right(1) elif data == 'turn left': - pass + self.robot.turn_left(1) elif data == 'faster': self.speed *= 2. elif data == 'slower': self.speed /= 2. - elif data: - pass - #print('I do not know the meaning of {}'.format(data)) + else: + self.robot.stop() -class BridgeSpeechRecognizerAckermannWheeledRobot(Bridge): +class BridgeSpeechRecognizerAckermannWheeledRobot(BridgeSpeechRecognizerWheeledRobot): r"""Bridge Speech Ackermann Wheeled Robot Bridge between the speech recognizer interface and a wheeled robot (with ackermann steering). You can give oral orders to the robot. """ - def __init__(self, interface, wheeled_robot, init_speed=1.): - if not isinstance(interface, SpeechRecognizerInterface): - raise TypeError("Expecting a speech recognizer interface") - if not isinstance(wheeled_robot, AckermannWheeledRobot): - raise TypeError("Expecting a wheeled robot of type Ackermann steering") - super(BridgeSpeechRecognizerAckermannWheeledRobot, self).__init__(interface) - self.robot = wheeled_robot - self.speed = init_speed + def __init__(self, robot, interface=None, speed=1., priority=None, verbose=False): + """ + Initialize the bridge between the speech recognizer interface and the wheeled robot. + + Args: + robot (AckermannWheeledRobot): wheeled robot instance. + interface (SpeechRecognizerInterface, None): speech recognizer interface. If None, it will instantiate it + here, and launch it in a thread. + speed (float): initial speed of the wheeled robot. + verbose (bool): If True, print information on the standard output. + """ + if not isinstance(robot, AckermannWheeledRobot): + raise TypeError("Expecting a wheeled robot of type Ackermann steering, instead got: {}".format(robot)) + + super(BridgeSpeechRecognizerAckermannWheeledRobot, self).__init__(robot=robot, interface=interface, + speed=speed, priority=priority, + verbose=verbose) self.steering_angle = 0. - def step(self): + def step(self, update_interface=False): + """Perform a step with the bridge.""" + super(BridgeSpeechRecognizerAckermannWheeledRobot, self).step(update_interface=update_interface) + + # get the data from the interface data = self.interface.data - #print('data: {}'.format(data)) - if data == 'stop': - self.robot.stop() - elif data == 'move forward': + + if self.verbose and data is not None: + print("Bridge: the data = {}".format(data)) + + if data == 'move forward': self.robot.drive_forward(self.speed) elif data == 'move backward': self.robot.drive_backward(self.speed) elif data == 'turn right': - self.robot.set_steering(np.deg2rad(-20)) + self.robot.steer(np.deg2rad(-20)) elif data == 'turn left': - self.robot.set_steering(np.deg2rad(20)) + self.robot.steer(np.deg2rad(20)) elif data == 'faster': self.speed *= 2. elif data == 'slower': self.speed /= 2. - elif data: - pass - #print('I do not know the meaning of {}'.format(data)) \ No newline at end of file + else: + self.robot.stop() diff --git a/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_quadcopter.py b/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_quadcopter.py index 50d0104..dee6525 100644 --- a/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_quadcopter.py +++ b/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_quadcopter.py @@ -67,16 +67,15 @@ class BridgeControllerQuadcopter(Bridge): "got: {}".format(type(interface))) # call superclass - super(BridgeControllerQuadcopter, self).__init__(interface, priority) + super(BridgeControllerQuadcopter, self).__init__(interface, priority=priority, verbose=verbose) # camera self.camera = camera - self.verbose = verbose self.fpv = first_person_view self.camera_pitch = self.camera.pitch # joystick threshold (to remove noise) - self.threshold = 0.05 + self.threshold = 0.1 ############## # Properties # @@ -144,8 +143,8 @@ class BridgeControllerQuadcopter(Bridge): def check_events(self): # move the quadcopter - left_joystick = self.interface.LJ # (x,y) - right_joystick = self.interface.RJ # (x,y) + left_joystick = self.interface.LJ[::-1] # (y,x) + right_joystick = self.interface.RJ[::-1] # (y,x) south_button = self.interface.BTN_SOUTH east_button = self.interface.BTN_EAST west_button = self.interface.BTN_WEST diff --git a/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py b/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py index a4861e5..c62f4c7 100644 --- a/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py +++ b/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py @@ -63,16 +63,15 @@ class BridgeControllerWheeledRobot(Bridge): raise TypeError # call superclass - super(BridgeControllerWheeledRobot, self).__init__(interface, priority) + super(BridgeControllerWheeledRobot, self).__init__(interface, priority=priority, verbose=verbose) # camera self.camera = camera - self.verbose = verbose self.fpv = first_person_view self.camera_pitch = self.camera.pitch # joystick threshold (to remove noise) - self.threshold = 0.05 + self.threshold = 0.1 ############## # Properties # @@ -139,28 +138,27 @@ class BridgeControllerWheeledRobot(Bridge): self.fpv = not self.fpv def check_key_events(self): - left_joystick = self.interface.LJ # (x,y) - # south_button = self.interface.BTN_SOUTH - # east_button = self.interface.BTN_EAST - # west_button = self.interface.BTN_WEST + left_joystick = self.interface.LJ[::-1] # (y,x) + south_button = self.interface.BTN_SOUTH + east_button = self.interface.BTN_EAST + west_button = self.interface.BTN_WEST # change camera view # if south_button: # self.change_camera_view() # change speed - # if east_button: - # self.speed += 1 - # if west_button: - # self.speed -= 1 + if east_button: + self.speed += 1 + if west_button: + self.speed -= 1 # move robot - # if np.linalg.norm(left_joystick) > self.threshold: - # # print(left_joystick) - # self.robot.move(velocity=self.speed * left_joystick) - # else: - # self.robot.move(velocity=[0., 0.]) - print(left_joystick[0]) + if np.linalg.norm(left_joystick) > self.threshold: + # print(left_joystick) + self.robot.move(velocity=self.speed * left_joystick) + else: + self.robot.move(velocity=[0., 0.]) class BridgeControllerDifferentialWheeledRobot(BridgeControllerWheeledRobot): @@ -199,7 +197,7 @@ class BridgeControllerDifferentialWheeledRobot(BridgeControllerWheeledRobot): def check_key_events(self): super(BridgeControllerDifferentialWheeledRobot, self).check_key_events() - directional_pad = self.interface.Dpad # (x,y) + directional_pad = self.interface.Dpad[::-1] # (y,x) # move robot if directional_pad[0] != 0: diff --git a/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_wheeled.py b/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_wheeled.py index 443c6c5..58959ff 100644 --- a/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_wheeled.py +++ b/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_wheeled.py @@ -45,6 +45,8 @@ class BridgeMouseKeyboardWheeledRobot(Bridge): * `left arrow`: turn/steer to the left * `right arrow`: turn/steer to the right * `space`: switch between first-person and third-person view. # TODO + * `shift`: increase speed + * `ctrl`: decrease speed * predefined in simulator: * `w`: show the wireframe (collision shapes) * `s`: show the reference system @@ -143,8 +145,8 @@ class BridgeMouseKeyboardWheeledRobot(Bridge): pitch, yaw = get_rpy_from_quaternion(self.robot.orientation)[1:] if self.fpv: # first-person view target_pos = self.robot.position + 2 * np.array([np.cos(yaw) * np.cos(pitch), - np.sin(yaw) * np.cos(pitch), - np.sin(pitch)]) + np.sin(yaw) * np.cos(pitch), + np.sin(pitch)]) self.camera.reset(distance=2, pitch=-pitch, yaw=yaw - np.pi / 2, target_position=target_pos) else: # third-person view self.camera.follow(body_id=self.robot.id, distance=2, yaw=yaw - np.pi / 2, pitch=self.camera_pitch) @@ -154,7 +156,16 @@ class BridgeMouseKeyboardWheeledRobot(Bridge): self.fpv = not self.fpv def check_key_events(self): - pass + key, pressed, down = self.interface.key, self.interface.key_pressed, self.interface.key_down + + # change camera view + # if key.space in pressed: + # self.change_camera_view() + + if key.shift in pressed: + self.speed += 1 + if key.ctrl in pressed: + self.speed -= 1 class BridgeMouseKeyboardDifferentialWheeledRobot(BridgeMouseKeyboardWheeledRobot): @@ -199,21 +210,18 @@ class BridgeMouseKeyboardDifferentialWheeledRobot(BridgeMouseKeyboardWheeledRobo verbose=verbose) def check_key_events(self): + super(BridgeMouseKeyboardDifferentialWheeledRobot, self).check_key_events() key, pressed, down = self.interface.key, self.interface.key_pressed, self.interface.key_down - # change camera view - # if key.space in pressed: - # self.change_camera_view() - # move the robot if key.top_arrow in down: self.robot.drive(speed=self.speed) elif key.bottom_arrow in down: self.robot.drive(speed=-self.speed) elif key.left_arrow in down: - self.robot.turn(speed=self.speed) + self.robot.turn(speed=self.speed/10.) elif key.right_arrow in down: - self.robot.turn(speed=-self.speed) + self.robot.turn(speed=-self.speed/10.) else: self.robot.drive(speed=0) @@ -261,10 +269,6 @@ class BridgeMouseKeyboardAckermannWheeledRobot(BridgeMouseKeyboardWheeledRobot): def check_key_events(self): key, pressed, down = self.interface.key, self.interface.key_pressed, self.interface.key_down - # change camera view - # if key.space in pressed: - # self.change_camera_view() - # move the robot if key.top_arrow in down: self.robot.drive(speed=self.speed) diff --git a/pyrobolearn/tools/interfaces/audio/speech.py b/pyrobolearn/tools/interfaces/audio/speech.py index cda2107..9766cb0 100644 --- a/pyrobolearn/tools/interfaces/audio/speech.py +++ b/pyrobolearn/tools/interfaces/audio/speech.py @@ -329,8 +329,8 @@ if __name__ == '__main__': print("Available languages are: {}".format(SpeechRecognizerInterface.available_languages)) - # interface = SpeechRecognizerInterface(verbose=True, lang='english') - interface = SpeechTranslatorInterface(verbose=True, from_lang='french', target_lang='english') + interface = SpeechRecognizerInterface(use_thread=True, verbose=True, lang='english') + # interface = SpeechTranslatorInterface(verbose=True, from_lang='french', target_lang='english') while True: data = interface.run() diff --git a/pyrobolearn/tools/interfaces/controllers/playstation.py b/pyrobolearn/tools/interfaces/controllers/playstation.py index d9d8759..db81e7a 100755 --- a/pyrobolearn/tools/interfaces/controllers/playstation.py +++ b/pyrobolearn/tools/interfaces/controllers/playstation.py @@ -402,7 +402,7 @@ class PS4ControllerInterface(PSControllerInterface): # Tests if __name__ == '__main__': # create controller - controller = PSControllerInterface(use_thread=True, sleep_dt=0.01) + controller = PSControllerInterface(use_thread=True) print(controller.map) print(controller.buttons) diff --git a/pyrobolearn/tools/interfaces/controllers/xbox.py b/pyrobolearn/tools/interfaces/controllers/xbox.py index 8ca648d..442eb34 100644 --- a/pyrobolearn/tools/interfaces/controllers/xbox.py +++ b/pyrobolearn/tools/interfaces/controllers/xbox.py @@ -398,12 +398,15 @@ if __name__ == '__main__': import time from itertools import count + # set variable + use_thread = True + # create interface - xbox = XboxOneControllerInterface() + xbox = XboxControllerInterface(use_thread=use_thread) for _ in count(): # run one step with the interface - xbox.run() # same as `step()` if we are not using threads + xbox.step() # same as `step()` if we are not using threads # get the last update and print it b = xbox.last_updated_button diff --git a/pyrobolearn/tools/interfaces/interface.py b/pyrobolearn/tools/interfaces/interface.py index bd14621..fbd097a 100644 --- a/pyrobolearn/tools/interfaces/interface.py +++ b/pyrobolearn/tools/interfaces/interface.py @@ -103,7 +103,7 @@ class Interface(object): # @thread_loop def run(self, *args, **kwargs): """ - Code to be run by the interface. This needs to be implemented by the user + Code to be run by the interface. This needs to be implemented by the user. """ pass diff --git a/pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py b/pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py index 0913924..46be688 100644 --- a/pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py +++ b/pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py @@ -71,7 +71,7 @@ class MouseKeyboardInterface(InputInterface): triggered, 4 if it has been released. """ - def __init__(self, simulator=None, verbose=False): + def __init__(self, simulator=None, verbose=False, *args, **kwargs): """ Initialize the Mouse-Keyboard Interface. This interface is a little bit special in the sense that we use the simulator to provide the mouse and keyboard events instead of using an external library. @@ -223,12 +223,12 @@ class MouseKeyboardInterface(InputInterface): # Tests if __name__ == '__main__': - from pyrobolearn.simulators import BulletSim import time from itertools import count + from pyrobolearn.simulators import Bullet # create simulator - sim = BulletSim() + sim = Bullet() # create interface interface = MouseKeyboardInterface(sim, verbose=True)