From c356c062bbffaae1231c543b3ef54ead9f408abe Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Fri, 21 Jun 2019 03:26:36 +0200 Subject: [PATCH] update game controllers and keyboard interfaces + example with quadcopter and wheeled robot --- examples/interfaces/mouse_keyboard.py | 6 +- examples/interfaces/playstation.py | 18 +- examples/robots/quadcopter_controller.py | 60 ++-- pyrobolearn/robots/epuck.py | 26 +- pyrobolearn/robots/f10_racecar.py | 2 +- pyrobolearn/robots/mkz.py | 2 +- pyrobolearn/robots/quadcopter.py | 5 +- pyrobolearn/robots/wheeled_robot.py | 103 +++++- pyrobolearn/simulators/simulator.py | 7 + .../robots/bridge_controller_quadcopter.py | 202 ++++++++++++ .../robots/bridge_controller_wheeled.py | 290 +++++++++++++---- .../bridge_mousekeyboard_quadcopter.py | 210 ++++++++++++ .../bridge_mousekeyboard_wheeled.py | 301 ++++++++++++++++++ .../interfaces/controllers/playstation.py | 249 ++++++++++++--- .../tools/interfaces/controllers/xbox.py | 120 ++++++- .../mouse_keyboard/mousekeyboard.py | 25 +- pyrobolearn/worlds/utils/heightmaps/gpr.py | 6 +- pyrobolearn/worlds/world.py | 7 +- 18 files changed, 1475 insertions(+), 164 deletions(-) create mode 100644 pyrobolearn/tools/bridges/controllers/robots/bridge_controller_quadcopter.py create mode 100644 pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_quadcopter.py create mode 100644 pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_wheeled.py diff --git a/examples/interfaces/mouse_keyboard.py b/examples/interfaces/mouse_keyboard.py index e5e3704..e110767 100644 --- a/examples/interfaces/mouse_keyboard.py +++ b/examples/interfaces/mouse_keyboard.py @@ -12,6 +12,8 @@ from pyrobolearn.tools.interfaces.mouse_keyboard import MouseKeyboardInterface sim = prl.simulators.Bullet() # create mouse keyboard interface +# Note that to give the simulator `sim` to the interface can be optional especially if there is only one simulator. +# The interface will look in the memory to check the instantiated simulators and take the first one if it exists. interface = MouseKeyboardInterface(sim) # run interface @@ -20,8 +22,10 @@ for _ in count(): interface.step() # print pressed keys + if len(interface.key_pressed) > 0: + print("Keys that are pressed: {}".format(interface.key_pressed)) if len(interface.key_down) > 0: - print("Keys that are pressed: {}".format(interface.key_down)) + print("Keys that are down: {}".format(interface.key_down)) # perform a step with the simulator sim.step(sleep_time=sim.dt) diff --git a/examples/interfaces/playstation.py b/examples/interfaces/playstation.py index c136aa7..fb7db5c 100644 --- a/examples/interfaces/playstation.py +++ b/examples/interfaces/playstation.py @@ -1,23 +1,33 @@ #!/usr/bin/env python """Load the Playstation game controller interface + +How to run: +``` +$ python playstation.py --help # for help +$ python playstation.py --controller ps # to use any PS game controller (by default) +$ python playstation.py --controller ps3 # to use PS3 game controller +$ python playstation.py --controller ps4 # to use PS4 game controller +``` """ import time from itertools import count import argparse -from pyrobolearn.tools.interfaces.controllers.playstation import PS3ControllerInterface, PS4ControllerInterface +from pyrobolearn.tools.interfaces.controllers.playstation import * # create parser to select the game controller parser = argparse.ArgumentParser() -parser.add_argument('-c', '--controller', help='The Playstation game controller to use (ps3 or ps4)', type=str, - choices=['ps3', 'ps4'], default='ps4') +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() # load corresponding Playstation controller interface +if args.controller == 'ps': + controller = PSControllerInterface(verbose=False) if args.controller == 'ps3': - controller = PS3ControllerInterface(verbose=True) + controller = PS3ControllerInterface(verbose=False) elif args.controller == 'ps4': controller = PS4ControllerInterface(verbose=False) else: diff --git a/examples/robots/quadcopter_controller.py b/examples/robots/quadcopter_controller.py index 5cece42..c1dde02 100644 --- a/examples/robots/quadcopter_controller.py +++ b/examples/robots/quadcopter_controller.py @@ -3,13 +3,25 @@ how to run: ``` -$ python quadcopter_controller.py --help # for help -$ python quadcopter_controller.py --controller xbox # to use Xbox game controller -$ python quadcopter_controller.py --controller ps3 # to use PS3 game controller +$ 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 """ -import numpy as np +# import numpy as np from itertools import count import argparse @@ -18,18 +30,26 @@ import pyrobolearn as prl # create parser to select the game controller parser = argparse.ArgumentParser() -parser.add_argument('-c', '--controller', help='the game controller to use', type=str, - choices=['xbox', 'ps3'], default='ps3') +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 == 'xbox': -# from pyrobolearn.tools.interfaces.controllers.xbox import Xbox360ControllerInterface as Controller -# elif args.controller == 'ps3': -# from pyrobolearn.tools.interfaces.controllers.playstation import PS3ControllerInterface as Controller -# else: -# raise NotImplementedError("Unknown game controller") +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 @@ -42,17 +62,17 @@ world = prl.worlds.BasicWorld(sim) robot = prl.robots.Quadcopter(sim, position=[0., 0., 2.]) world.load_robot(robot) -# load interface -# controller = Controller() +# load interface that accepts input events +controller = Controller() + +# 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(): - # robot.hover() - # robot.set_propeller_velocities(velocity) - robot.move([1., 1., 1.]) - - # follow quadcopter (seen from behind) - world.follow(robot, distance=2, yaw=-np.pi / 2) + # 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/robots/epuck.py b/pyrobolearn/robots/epuck.py index d89425d..0f7d5a3 100644 --- a/pyrobolearn/robots/epuck.py +++ b/pyrobolearn/robots/epuck.py @@ -49,15 +49,33 @@ class Epuck(DifferentialWheeledRobot): if link in self.link_names] self.wheel_directions = np.ones(len(self.wheels)) + def turn(self, speed): + """Turn the robot. If the speed is positive, turn to the left, otherwise turn to the right (using the + right-hand rule). + + Args: + speed (float): speed to turn to the left (if speed is positive) or to the right (if speed is negative). + """ + self.set_joint_velocities(speed * np.array([-1, 1]), self.wheels) + + def move(self, velocity): + """Move the robot at the specified 2D velocity vector. + + Args: + velocity (np.array[2]): 2D velocity vector defined in the xy plane. The magnitude represents the speed. + """ + velocities = np.array([velocity[0] + velocity[1], velocity[0] - velocity[1]]) + self.set_joint_velocities(velocities=velocities) + # Test if __name__ == "__main__": from itertools import count - from pyrobolearn.simulators import BulletSim + from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld # Create simulator - sim = BulletSim() + sim = Bullet() # create world world = BasicWorld(sim) @@ -79,5 +97,7 @@ if __name__ == "__main__": for _ in count(): # robots[0].update_joint_slider() for robot in robots: - robot.drive(5) + # robot.drive(5) + # robot.turn(5) + robot.move([0., 1.]) world.step(sleep_dt=1./240) diff --git a/pyrobolearn/robots/f10_racecar.py b/pyrobolearn/robots/f10_racecar.py index 6834f82..1b65370 100644 --- a/pyrobolearn/robots/f10_racecar.py +++ b/pyrobolearn/robots/f10_racecar.py @@ -51,7 +51,7 @@ class F10Racecar(AckermannWheeledRobot): self.steering = [self.get_link_ids(link) for link in ['left_steering_hinge', 'right_steering_hinge'] if link in self.link_names] - def set_steering(self, angle): + def steer(self, angle): """Set steering angle""" angle = angle * np.ones(len(self.steering)) self.set_joint_positions(angle, joint_ids=self.steering) diff --git a/pyrobolearn/robots/mkz.py b/pyrobolearn/robots/mkz.py index f3d82c5..469b69b 100644 --- a/pyrobolearn/robots/mkz.py +++ b/pyrobolearn/robots/mkz.py @@ -54,7 +54,7 @@ class MKZ(AckermannWheeledRobot): self.steering = [self.get_link_ids(link) for link in ['steer_fl', 'steer_fr'] if link in self.link_names] - def set_steering(self, angle): + def steer(self, angle): """Set steering angle""" angle = angle * np.ones(len(self.steering)) self.set_joint_positions(angle, joint_ids=self.steering) diff --git a/pyrobolearn/robots/quadcopter.py b/pyrobolearn/robots/quadcopter.py index cdd0bc8..8ef141e 100644 --- a/pyrobolearn/robots/quadcopter.py +++ b/pyrobolearn/robots/quadcopter.py @@ -9,6 +9,7 @@ from pyrobolearn.robots.uav import RotaryWingUAV from pyrobolearn.utils.transformation import get_matrix_from_quaternion from pyrobolearn.utils.units import inches_to_meters, rpm_to_rad_per_second, rad_per_second_to_rpm + __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __license__ = "GNU GPLv3" @@ -278,7 +279,7 @@ class Quadcopter(RotaryWingUAV): """Turn the quadcopter to the left. Args: - speed (float): speed to turn to the left. + speed (float): positive speed to turn to the left. """ velocities = self.stationary_velocities + speed * np.array([0, 1., 0., 1.]) * self.propeller_directions self.set_propeller_velocities(velocities) @@ -287,7 +288,7 @@ class Quadcopter(RotaryWingUAV): """Turn the quadcopter to the right. Args: - speed (float): speed to turn to the right. + speed (float): positive speed to turn to the right. """ velocities = self.stationary_velocities + speed * np.array([1., 0., 1., 0.]) * self.propeller_directions self.set_propeller_velocities(velocities) diff --git a/pyrobolearn/robots/wheeled_robot.py b/pyrobolearn/robots/wheeled_robot.py index 4d78958..bdb6a5d 100644 --- a/pyrobolearn/robots/wheeled_robot.py +++ b/pyrobolearn/robots/wheeled_robot.py @@ -6,6 +6,7 @@ import numpy as np from pyrobolearn.robots.robot import Robot + __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] @@ -68,6 +69,14 @@ class WheeledRobot(Robot): return wheel_ids return self.wheels + def move(self, velocity): + """Move the robot at the specified 2D velocity vector. + + Args: + velocity (np.array[2]): 2D velocity vector defined in the xy plane. The magnitude represents the speed. + """ + pass + def drive(self, speed): if isinstance(speed, (int, float)): speed = speed * np.ones(self.num_wheels) @@ -83,12 +92,6 @@ class WheeledRobot(Robot): def drive_backward(self, speed): self.drive(-speed) - def turn_right(self): - pass - - def turn_left(self): - pass - class DifferentialWheeledRobot(WheeledRobot): r"""Differential Wheeled Robot @@ -100,18 +103,62 @@ class DifferentialWheeledRobot(WheeledRobot): Check also [2] for the different types of drive. + The kinematics of these kind of platforms (with two wheels) can be described mathematically by [4]: + + .. math:: + + v &= \frac{r (\omega_R + \omega_L)}{2} \\ + \omega &= \frac{r (\omega_R - \omega_L)}{d} + + where :math:`\omega_R` (resp. :math:`\omega_L`) is the angular velocity of the right (resp. left) wheel, + :math:`v` is the driving velocity of the platform, :math:`\omega` is its steering velocity, :math:`r` is the + radius of the wheels and :math:`d` is the distance between their centers. + + This formulation is equivalent to: + + .. math:: + + \omega_R &= v + \frac{d}{2r} \omega \\ + \omega_L &= v - \frac{d}{2r} \omega + References: [1] Wikipedia: https://en.wikipedia.org/wiki/Differential_wheeled_robot [2] "Pros and cons for different types of drive selection": https://robohub.org/pros-and-cons-for-different-types-of-drive-selection/ [3] Wheel Control Theory: http://www.robotplatform.com/knowledge/Classification_of_Robots/wheel_control_theory.html + [4] "Robotics: Modelling, Planning and Control" (section 11.2), Siciliano et al., 2010 """ def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scale=1.): super(DifferentialWheeledRobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scale) + def turn(self, speed): + """Turn the robot. If the speed is positive, turn to the left, otherwise turn to the right (using the + right-hand rule). + + Args: + speed (float): speed to turn to the left (if speed is positive) or to the right (if speed is negative). + """ + pass + + def turn_right(self, speed): + """Turn the quadcopter to the right. + + Args: + speed (float): positive speed to turn to the right. + """ + self.turn(speed) + + def turn_left(self, speed): + """Turn the robot to the left. + + Args: + speed (float): positive speed to turn to the left. + """ + self.turn(-speed) + class AckermannWheeledRobot(WheeledRobot): r"""Ackermann steering Wheeled Robot @@ -136,6 +183,46 @@ class AckermannWheeledRobot(WheeledRobot): self.steering = 0 # id of steering joint - def set_steering(self, angle): - """Set steering angle""" + def move(self, velocity): + """Move the robot at the specified 2D velocity vector. + + Args: + velocity (np.array[2]): 2D velocity vector defined in the xy plane. The magnitude represents the speed. + """ + if velocity[0] > 0: # forward + angle = np.arctan2(velocity[1], velocity[0]) + magnitude = np.linalg.norm(velocity) + self.steer(angle) + self.drive_forward(magnitude) + else: # backward + angle = np.arctan2(velocity[1], -velocity[0]) + magnitude = np.linalg.norm(velocity) + self.steer(angle) + self.drive_backward(magnitude) + + def steer(self, angle): + """Set steering angle. If the angle is positive, turn to the left, otherwise turn to the right (using the + right-hand rule). + + Args: + angle (float): steering angle. If the angle is positive, steer to the left, otherwise, steer to the right. + """ pass + + def steer_left(self, angle): + """ + Steer to the left at the specified angle. + + Args: + angle (float): positive steering angle. + """ + self.steer(angle) + + def steer_right(self, angle): + """ + Steer to the right at the specified angle. + + Args: + angle (float): positive steering angle. + """ + self.steer(-angle) diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py index 805f4b0..1fa6be6 100644 --- a/pyrobolearn/simulators/simulator.py +++ b/pyrobolearn/simulators/simulator.py @@ -17,6 +17,7 @@ References: [3] PEP8: https://www.python.org/dev/peps/pep-0008/ """ +from pyrobolearn.utils.data_structures.orderedset import OrderedSet __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" @@ -45,6 +46,9 @@ class Simulator(object): [2] PEP8: https://www.python.org/dev/peps/pep-0008/ """ + # keep track of the instantiated simulators + instances = OrderedSet() + # TODO: this is really bad to have attributes like that... It doesn't generalize well to other simulators... B3G_ALT = 65308 @@ -188,6 +192,9 @@ class Simulator(object): self.default_timestep = 1. / 240 self.dt = self.default_timestep + # add instance to the set of all instantiated simulators + self.__class__.instances.add(self) + # TODO: this is really bad to have attributes like that... It doesn't generalize well to other simulators... # import pybullet # for attribute in dir(pybullet): diff --git a/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_quadcopter.py b/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_quadcopter.py new file mode 100644 index 0000000..50d0104 --- /dev/null +++ b/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_quadcopter.py @@ -0,0 +1,202 @@ +#!/usr/bin/env python +"""Define the Bridge between the game controller interface and the world. + +Dependencies: +- `pyrobolearn.tools.interfaces.controllers.controller` +- `pyrobolearn.tools.bridges.Bridge` +""" + +import numpy as np + +from pyrobolearn.tools.interfaces.controllers.controller import GameControllerInterface +from pyrobolearn.tools.bridges import Bridge +from pyrobolearn.robots.quadcopter import Quadcopter +from pyrobolearn.worlds.world_camera import WorldCamera + +from pyrobolearn.utils.transformation import get_rpy_from_quaternion + + +__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 BridgeControllerQuadcopter(Bridge): + r"""Bridge between game Controller and Quadcopter + + Bridge between the game controller and a quadcopter robot. + + Here is the 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 + """ + + def __init__(self, quadcopter, interface, camera=None, first_person_view=False, speed=10, + priority=None, verbose=False): + """ + Initialize the Bridge between a game controller interface and a quadcopter. + + Args: + quadcopter (Quadcopter): quadcopter robot instance. + interface (None, GameControllerInterface): game controller interface. + camera (WorldCamera): world camera instance. This will allow the user to switch between first-person and + third-person view. If None, it will create an instance of it. + first_person_view (bool): if True, it will set the world camera to the first person view. If False, it + will be the third-person view. + speed (float): speed of the propeller + priority (int): priority of the bridge. + verbose (bool): If True, print information on the standard output. + """ + # set quadcopter + self.quadcopter = quadcopter + if speed == 0: + speed = 1 + self.speed = speed if speed > 0 else -speed + + # check interface + if not isinstance(interface, GameControllerInterface): + raise TypeError("Expecting the given 'interface' to be an instance of `GameControllerInterface`, instead " + "got: {}".format(type(interface))) + + # call superclass + super(BridgeControllerQuadcopter, self).__init__(interface, priority) + + # 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 + + ############## + # Properties # + ############## + + @property + def quadcopter(self): + """Return the quadcopter instance.""" + return self._quadcopter + + @quadcopter.setter + def quadcopter(self, quadcopter): + """Set the quadcopter instance.""" + if not isinstance(quadcopter, Quadcopter): + raise TypeError("Expecting the given 'quadcopter' to be an instance of `Quadcopter`, instead got: " + "{}".format(type(quadcopter))) + self._quadcopter = quadcopter + + @property + def simulator(self): + """Return the simulator instance.""" + return self._quadcopter.simulator + + @property + def camera(self): + """Return the world camera instance.""" + return self._camera + + @camera.setter + def camera(self, camera): + """Set the world camera instance.""" + if camera is None: + camera = WorldCamera(self.simulator) + elif not isinstance(camera, WorldCamera): + raise TypeError("Expecting the given 'camera' to be an instance of `WorldCamera`, instead got: " + "{}".format(type(camera))) + self._camera = camera + + ########### + # Methods # + ########### + + def step(self, update_interface=False): + """Perform a step: map the mouse-keyboard interface to the world""" + # update interface + if update_interface: + self.interface() + + # check interface events + self.check_events() + + # set camera view + pitch, yaw = get_rpy_from_quaternion(self.quadcopter.orientation)[1:] + if self.fpv: # first-person view + target_pos = self.quadcopter.position + 2 * np.array([np.cos(yaw) * np.cos(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.quadcopter.id, distance=2, yaw=yaw - np.pi / 2, pitch=self.camera_pitch) + + def change_camera_view(self): + """Change camera view between first-person view and third-person view.""" + self.fpv = not self.fpv + + def check_events(self): + # move the quadcopter + left_joystick = self.interface.LJ # (x,y) + right_joystick = self.interface.RJ # (x,y) + south_button = self.interface.BTN_SOUTH + east_button = self.interface.BTN_EAST + west_button = self.interface.BTN_WEST + left_norm, right_norm = np.linalg.norm(left_joystick), np.linalg.norm(right_joystick) + + # change camera view + if south_button: + self.change_camera_view() + + # change speed + if east_button: + self.speed += 1 + if west_button: + self.speed -= 1 + + # move the quadcopter + if left_norm > self.threshold: # left joystick + if right_norm > self.threshold: # with right joystick + velocity = self.speed * np.array([left_joystick[1], left_joystick[0], right_joystick[1]]) + else: + velocity = self.speed * np.array([left_joystick[1], left_joystick[0], 0.]) + self.quadcopter.move(velocity=velocity) + elif right_norm > self.threshold: # right joystick + if np.abs(right_joystick[1]) > np.abs(right_joystick[0]): + self.quadcopter.move(velocity=self.speed * np.array([0, 0, right_joystick[1]])) + else: + self.quadcopter.turn(speed=-1 * right_joystick[0]) + else: + self.quadcopter.hover() + + +# Tests +if __name__ == '__main__': + from itertools import count + import pyrobolearn as prl + from pyrobolearn.tools.interfaces.controllers.playstation import PSControllerInterface + + # create simulator + sim = prl.simulators.Bullet() + + # create World + world = prl.worlds.BasicWorld(sim) + + # load robot + # robot = world.load_robot('quadcopter') + robot = Quadcopter(sim, position=[0, 0, 2.]) + + # create bridge/interface + controller = PSControllerInterface(use_thread=True, sleep_dt=0.01) + bridge = BridgeControllerQuadcopter(robot, interface=controller, verbose=True) + + for _ in count(): + bridge.step(update_interface=False) # when using thread for the interface, it updates itself automatically + world.step(sleep_dt=sim.dt) diff --git a/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py b/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py index 7363c35..a4861e5 100644 --- a/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py +++ b/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py @@ -2,9 +2,15 @@ """Bridges between controller interface and wheeled robots """ -from pyrobolearn.robots import WheeledRobot, AckermannWheeledRobot -from pyrobolearn.tools.interfaces.controllers.xbox import XboxControllerInterface, XboxOneControllerInterface +from abc import ABCMeta +import numpy as np + +from pyrobolearn.tools.interfaces.controllers.controller import GameControllerInterface from pyrobolearn.tools.bridges.bridge import Bridge +from pyrobolearn.robots.wheeled_robot import WheeledRobot, DifferentialWheeledRobot, AckermannWheeledRobot +from pyrobolearn.worlds.world_camera import WorldCamera + +from pyrobolearn.utils.transformation import get_rpy_from_quaternion __author__ = "Brian Delhaisse" @@ -17,74 +23,246 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" -class BridgeXboxWheeledRobot(Bridge): - r"""Bridge Xbox Wheeled Robot +class BridgeControllerWheeledRobot(Bridge): + r"""Bridge between GameController and a wheeled robot - Bridge between the Xbox controller interface and a wheeled robot. You can move the robot using the joystick. + Bridge between the game controller and a wheeled robot. + + Here is the mapping between the controller and the robot: + - left joystick: velocity of the wheeled robot + - south button (X on PlayStation and A on Xbox): change between the first-person and third-person view # TODO + - east button (circle on PlayStation and B on Xbox): increase the speed # TODO + - west button (square on PlayStation and X on Xbox): decrease the speed # TODO + """ + __metaclass__ = ABCMeta + + def __init__(self, robot, interface=None, camera=None, first_person_view=False, speed=10, + priority=None, verbose=False): + """ + Initialize the Bridge between a game controller interface and a wheeled robot instance. + + Args: + robot (WheeledRobot): wheeled robot instance. + interface (GameControllerInterface): game controller interface. + camera (WorldCamera): world camera instance. This will allow the user to switch between first-person and + third-person view. If None, it will create an instance of it. + first_person_view (bool): if True, it will set the world camera to the first person view. If False, it + will be the third-person view. + speed (float): speed of the propeller + priority (int): priority of the bridge. + verbose (bool): If True, print information on the standard output. + """ + # set robot + self.robot = robot + if speed == 0: + speed = 1 + self.speed = speed if speed > 0 else -speed + + # if interface not defined, create one. + if not isinstance(interface, GameControllerInterface): + raise TypeError + + # call superclass + super(BridgeControllerWheeledRobot, self).__init__(interface, priority) + + # 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 + + ############## + # Properties # + ############## + + @property + def robot(self): + """Return the wheeled robot instance.""" + return self._robot + + @robot.setter + def robot(self, robot): + """Set the wheeled robot instance.""" + if not isinstance(robot, WheeledRobot): + raise TypeError("Expecting the given 'robot' to be an instance of `WheeledRobot`, instead got: " + "{}".format(type(robot))) + self._robot = robot + + @property + def simulator(self): + """Return the simulator instance.""" + return self._robot.simulator + + @property + def camera(self): + """Return the world camera instance.""" + return self._camera + + @camera.setter + def camera(self, camera): + """Set the world camera instance.""" + if camera is None: + camera = WorldCamera(self.simulator) + elif not isinstance(camera, WorldCamera): + raise TypeError("Expecting the given 'camera' to be an instance of `WorldCamera`, instead got: " + "{}".format(type(camera))) + self._camera = camera + + ########### + # Methods # + ########### + + def step(self, update_interface=False): + """Perform a step: map the mouse-keyboard interface to the world""" + # update interface + if update_interface: + self.interface() + + # check keyboard events + self.check_key_events() + + # set camera view + 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)]) + 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) + + def change_camera_view(self): + """Change camera view between first-person view and third-person view.""" + 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 + + # change camera view + # if south_button: + # self.change_camera_view() + + # change speed + # 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]) + + +class BridgeControllerDifferentialWheeledRobot(BridgeControllerWheeledRobot): + r"""Bridge between the mouse-keyboard and a differential wheeled robot. + + Here is the mapping between the controller and the robot: + - left joystick: velocity of the wheeled robot + - 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 """ - def __init__(self, interface, wheeled_robot): - # quick checks - if not isinstance(interface, XboxControllerInterface): - raise TypeError("Expecting a speech recognizer interface") - if not isinstance(wheeled_robot, WheeledRobot): - raise TypeError("Expecting a wheeled robot") + def __init__(self, robot, interface=None, camera=None, first_person_view=False, speed=10, priority=None, + verbose=False): + """ + Initialize the Bridge between a game controller interface and a differential wheeled robot instance. - # call super class - super(BridgeXboxWheeledRobot, self).__init__(interface) - self.robot = wheeled_robot - self.speed = 1. + Args: + robot (AckermannWheeledRobot): wheeled robot instance. + interface (GameControllerInterface): game controller interface. + camera (WorldCamera): world camera instance. This will allow the user to switch between first-person and + third-person view. If None, it will create an instance of it. + first_person_view (bool): if True, it will set the world camera to the first person view. If False, it + will be the third-person view. + speed (float): speed of the propeller + priority (int): priority of the bridge. + verbose (bool): If True, print information on the standard output. + """ + if not isinstance(robot, DifferentialWheeledRobot): + raise TypeError("Expecting the given 'robot' to be an instance of `DifferentialWheeledRobot`, instead " + "got: {}".format(type(robot))) + super(BridgeControllerDifferentialWheeledRobot, self).__init__(robot, interface=interface, camera=camera, + first_person_view=first_person_view, + speed=speed, priority=priority, + verbose=verbose) - def step(self): - x, y = self.interface.LJ + def check_key_events(self): + super(BridgeControllerDifferentialWheeledRobot, self).check_key_events() + directional_pad = self.interface.Dpad # (x,y) + + # move robot + if directional_pad[0] != 0: + self.robot.turn(directional_pad[0]) + if directional_pad[1] != 0: + self.robot.drive_forward(directional_pad[1] * self.speed) -class BridgeXboxOneWheeledRobot(Bridge): - r"""Bridge Xbox Wheeled Robot +class BridgeControllerAckermannWheeledRobot(BridgeControllerWheeledRobot): + r"""Bridge between the mouse-keyboard and a Ackermann wheeled robot. - Bridge between the Xbox One controller interface and a wheeled robot. You can move the robot using the joystick. + Here is the mapping between the controller and the robot: + - left joystick: velocity of the wheeled robot + - 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 """ - def __init__(self, interface, wheeled_robot): - # quick check - if not isinstance(interface, XboxOneControllerInterface): - raise TypeError("Expecting a speech recognizer interface") - if not isinstance(wheeled_robot, WheeledRobot): - raise TypeError("Expecting a wheeled robot") + def __init__(self, robot, interface, camera=None, first_person_view=False, speed=10, priority=None, + verbose=False): + """ + Initialize the Bridge between a game controller interface and a wheeled robot instance. - super(BridgeXboxOneWheeledRobot, self).__init__(interface) - self.robot = wheeled_robot - self.speed = 1. - - def step(self): - x, y = self.interface.LJ + Args: + robot (DifferentialWheeledRobot): wheeled robot instance. + interface (GameControllerInterface): game controller interface. + camera (WorldCamera): world camera instance. This will allow the user to switch between first-person and + third-person view. If None, it will create an instance of it. + first_person_view (bool): if True, it will set the world camera to the first person view. If False, it + will be the third-person view. + speed (float): speed of the propeller + priority (int): priority of the bridge. + verbose (bool): If True, print information on the standard output. + """ + if not isinstance(robot, AckermannWheeledRobot): + raise TypeError("Expecting the given 'robot' to be an instance of `AckermannWheeledRobot`, instead got: " + "{}".format(type(robot))) + super(BridgeControllerAckermannWheeledRobot, self).__init__(robot, interface=interface, camera=camera, + first_person_view=first_person_view, + speed=speed, priority=priority, verbose=verbose) -class BridgeXboxOneAckermannWheeledRobot(Bridge): - r"""Bridge Xbox One Ackermann Wheeled Robot +# Tests +if __name__ == '__main__': + from itertools import count + import pyrobolearn as prl + from pyrobolearn.tools.interfaces.controllers.playstation import PSControllerInterface - Bridge between the Xbox One controller interface and a wheeled robot. You can move the robot using the joystick. - """ + # create simulator + sim = prl.simulators.Bullet() - def __init__(self, interface, wheeled_robot): - if not isinstance(interface, XboxOneControllerInterface): - raise TypeError("Expecting a speech recognizer interface") - if not isinstance(wheeled_robot, AckermannWheeledRobot): - raise TypeError("Expecting a wheeled robot") - super(BridgeXboxOneAckermannWheeledRobot, self).__init__(interface) - self.robot = wheeled_robot - self.speed = 1. + # create World + world = prl.worlds.BasicWorld(sim) - def step(self): - x, y = self.interface.LJ - self.robot.set_steering(-x / 2.) - self.robot.drive_forward(y * self.speed) + # load robot + # robot = world.load_robot('epuck') + robot = prl.robots.Epuck(sim) - if self.interface.A: - print('increasing speed +1') - self.speed += 1. - if self.interface.B: - print('decreasing speed -1') - self.speed -= 1. - if self.speed < 1.: - self.speed = 1. + # create bridge/interface + interface = PSControllerInterface(use_thread=True, sleep_dt=0.01) + bridge = BridgeControllerWheeledRobot(robot, interface=interface, verbose=True) + + # run simulator + for _ in count(): + bridge.step(update_interface=False) # when using thread for the interface, it updates itself automatically + world.step(sleep_dt=sim.dt) diff --git a/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_quadcopter.py b/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_quadcopter.py new file mode 100644 index 0000000..7fac004 --- /dev/null +++ b/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_quadcopter.py @@ -0,0 +1,210 @@ +#!/usr/bin/env python +"""Define the Bridge between the mouse-keyboard interface and the world. + +Dependencies: +- `pyrobolearn.tools.interfaces.MouseKeyboardInterface` +- `pyrobolearn.tools.bridges.Bridge` +""" + +import numpy as np + +from pyrobolearn.tools.interfaces import MouseKeyboardInterface +from pyrobolearn.tools.bridges import Bridge +from pyrobolearn.robots.quadcopter import Quadcopter +from pyrobolearn.worlds.world_camera import WorldCamera + +from pyrobolearn.utils.transformation import get_rpy_from_quaternion + + +__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 BridgeMouseKeyboardQuadcopter(Bridge): + r"""Bridge between MouseKeyboard and Quadcopter + + Bridge between the mouse-keyboard and a quadcopter robot. + + Mouse: + * predefined in simulator: + * `scroll wheel`: zoom + * `ctrl`/`alt` + `scroll button`: move the camera using the mouse + * `ctrl`/`alt` + `left-click`: rotate the camera using the mouse + * `left-click` and drag: transport the object + + Keyboard: + * `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. + * predefined in simulator: + * `w`: show the wireframe (collision shapes) + * `s`: show the reference system + * `v`: show bounding boxes + * `g`: show/hide parts of the GUI the side columns + * `esc`: quit the simulator + """ + + def __init__(self, quadcopter, interface=None, camera=None, first_person_view=False, speed=10, + priority=None, verbose=False): + """ + Initialize the Bridge between a Mouse-Keyboard interface and a quadcopter. + + Args: + quadcopter (Quadcopter): quadcopter robot instance. + interface (None, MouseKeyboardInterface): mouse keyboard interface. If None, it will create one. + camera (WorldCamera): world camera instance. This will allow the user to switch between first-person and + third-person view. If None, it will create an instance of it. + first_person_view (bool): if True, it will set the world camera to the first person view. If False, it + will be the third-person view. + speed (float): speed of the propeller + priority (int): priority of the bridge. + verbose (bool): If True, print information on the standard output. + """ + # set quadcopter + self.quadcopter = quadcopter + if speed == 0: + speed = 1 + self.speed = speed if speed > 0 else -speed + + # if interface not defined, create one. + if not isinstance(interface, MouseKeyboardInterface): + interface = MouseKeyboardInterface(self.simulator) + + # call superclass + super(BridgeMouseKeyboardQuadcopter, self).__init__(interface, priority) + + # camera + self.camera = camera + self.verbose = verbose + self.fpv = first_person_view + self.camera_pitch = self.camera.pitch + + ############## + # Properties # + ############## + + @property + def quadcopter(self): + """Return the quadcopter instance.""" + return self._quadcopter + + @quadcopter.setter + def quadcopter(self, quadcopter): + """Set the quadcopter instance.""" + if not isinstance(quadcopter, Quadcopter): + raise TypeError("Expecting the given 'quadcopter' to be an instance of `Quadcopter`, instead got: " + "{}".format(type(quadcopter))) + self._quadcopter = quadcopter + + @property + def simulator(self): + """Return the simulator instance.""" + return self._quadcopter.simulator + + @property + def camera(self): + """Return the world camera instance.""" + return self._camera + + @camera.setter + def camera(self, camera): + """Set the world camera instance.""" + if camera is None: + camera = WorldCamera(self.simulator) + elif not isinstance(camera, WorldCamera): + raise TypeError("Expecting the given 'camera' to be an instance of `WorldCamera`, instead got: " + "{}".format(type(camera))) + self._camera = camera + + ########### + # Methods # + ########### + + def step(self, update_interface=False): + """Perform a step: map the mouse-keyboard interface to the world""" + # update interface + if update_interface: + self.interface() + + # check keyboard events + self.check_key_events() + + # set camera view + pitch, yaw = get_rpy_from_quaternion(self.quadcopter.orientation)[1:] + if self.fpv: # first-person view + target_pos = self.quadcopter.position + 2 * np.array([np.cos(yaw) * np.cos(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.quadcopter.id, distance=2, yaw=yaw - np.pi / 2, pitch=self.camera_pitch) + + def change_camera_view(self): + """Change camera view between first-person view and third-person view.""" + self.fpv = not self.fpv + + 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 quadcopter + if key.ctrl in down: + if key.top_arrow in down: + self.quadcopter.ascend(speed=10 * self.speed) + elif key.bottom_arrow in down: + self.quadcopter.descend(speed=10 * self.speed) + elif key.left_arrow in down: + self.quadcopter.turn_left(speed=self.speed) + elif key.right_arrow in down: + self.quadcopter.turn_right(speed=self.speed) + else: + self.quadcopter.hover() + else: + if key.top_arrow in down: + self.quadcopter.move_forward(speed=self.speed) + elif key.bottom_arrow in down: + self.quadcopter.move_backward(speed=self.speed) + elif key.left_arrow in down: + self.quadcopter.move_left(speed=self.speed) + elif key.right_arrow in down: + self.quadcopter.move_right(speed=self.speed) + else: + self.quadcopter.hover() + + +# Tests +if __name__ == '__main__': + from itertools import count + import pyrobolearn as prl + + # create simulator + sim = prl.simulators.Bullet() + + # create World + world = prl.worlds.BasicWorld(sim) + + # load robot + # robot = world.load_robot('quadcopter') + robot = Quadcopter(sim, position=[0, 0, 1.]) + + # create bridge/interface + bridge = BridgeMouseKeyboardQuadcopter(robot, verbose=True) + + for _ in count(): + bridge.step(update_interface=True) + world.step(sleep_dt=sim.dt) diff --git a/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_wheeled.py b/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_wheeled.py new file mode 100644 index 0000000..443c6c5 --- /dev/null +++ b/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_wheeled.py @@ -0,0 +1,301 @@ +#!/usr/bin/env python +"""Define the Bridge between the mouse-keyboard interface and the world. + +Dependencies: +- `pyrobolearn.tools.interfaces.MouseKeyboardInterface` +- `pyrobolearn.tools.bridges.Bridge` +""" + +from abc import ABCMeta +import numpy as np + +from pyrobolearn.tools.interfaces import MouseKeyboardInterface +from pyrobolearn.tools.bridges import Bridge +from pyrobolearn.robots.wheeled_robot import WheeledRobot, DifferentialWheeledRobot, AckermannWheeledRobot +from pyrobolearn.worlds.world_camera import WorldCamera + +from pyrobolearn.utils.transformation import get_rpy_from_quaternion + + +__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 BridgeMouseKeyboardWheeledRobot(Bridge): + r"""Bridge between MouseKeyboard and a wheeled robot + + Bridge between the mouse-keyboard and a wheeled robot. + + Mouse: + * predefined in simulator: + * `scroll wheel`: zoom + * `ctrl`/`alt` + `scroll button`: move the camera using the mouse + * `ctrl`/`alt` + `left-click`: rotate the camera using the mouse + * `left-click` and drag: transport the object + + Keyboard: + * `top arrow`: move forward + * `bottom arrow`: move backward + * `left arrow`: turn/steer to the left + * `right arrow`: turn/steer to the right + * `space`: switch between first-person and third-person view. # TODO + * predefined in simulator: + * `w`: show the wireframe (collision shapes) + * `s`: show the reference system + * `v`: show bounding boxes + * `g`: show/hide parts of the GUI the side columns + * `esc`: quit the simulator + """ + __metaclass__ = ABCMeta + + def __init__(self, robot, interface=None, camera=None, first_person_view=False, speed=10, + priority=None, verbose=False): + """ + Initialize the Bridge between a Mouse-Keyboard interface and a wheeled robot instance. + + Args: + robot (WheeledRobot): wheeled robot instance. + interface (None, MouseKeyboardInterface): mouse keyboard interface. If None, it will create one. + camera (WorldCamera): world camera instance. This will allow the user to switch between first-person and + third-person view. If None, it will create an instance of it. + first_person_view (bool): if True, it will set the world camera to the first person view. If False, it + will be the third-person view. + speed (float): speed of the propeller + priority (int): priority of the bridge. + verbose (bool): If True, print information on the standard output. + """ + # set robot + self.robot = robot + if speed == 0: + speed = 1 + self.speed = speed if speed > 0 else -speed + + # if interface not defined, create one. + if not isinstance(interface, MouseKeyboardInterface): + interface = MouseKeyboardInterface(self.simulator) + + # call superclass + super(BridgeMouseKeyboardWheeledRobot, self).__init__(interface, priority) + + # camera + self.camera = camera + self.verbose = verbose + self.fpv = first_person_view + self.camera_pitch = self.camera.pitch + + ############## + # Properties # + ############## + + @property + def robot(self): + """Return the wheeled robot instance.""" + return self._robot + + @robot.setter + def robot(self, robot): + """Set the wheeled robot instance.""" + if not isinstance(robot, WheeledRobot): + raise TypeError("Expecting the given 'robot' to be an instance of `WheeledRobot`, instead got: " + "{}".format(type(robot))) + self._robot = robot + + @property + def simulator(self): + """Return the simulator instance.""" + return self._robot.simulator + + @property + def camera(self): + """Return the world camera instance.""" + return self._camera + + @camera.setter + def camera(self, camera): + """Set the world camera instance.""" + if camera is None: + camera = WorldCamera(self.simulator) + elif not isinstance(camera, WorldCamera): + raise TypeError("Expecting the given 'camera' to be an instance of `WorldCamera`, instead got: " + "{}".format(type(camera))) + self._camera = camera + + ########### + # Methods # + ########### + + def step(self, update_interface=False): + """Perform a step: map the mouse-keyboard interface to the world""" + # update interface + if update_interface: + self.interface() + + # check keyboard events + self.check_key_events() + + # set camera view + 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)]) + 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) + + def change_camera_view(self): + """Change camera view between first-person view and third-person view.""" + self.fpv = not self.fpv + + def check_key_events(self): + pass + + +class BridgeMouseKeyboardDifferentialWheeledRobot(BridgeMouseKeyboardWheeledRobot): + r"""Bridge between the mouse-keyboard and a differential wheeled robot. + + Keyboard: + * `top arrow`: move forward + * `bottom arrow`: move backward + * `left arrow`: turn to the left + * `right arrow`: turn to the right + * `space`: switch between first-person and third-person view. + * predefined in simulator: + * `w`: show the wireframe (collision shapes) + * `s`: show the reference system + * `v`: show bounding boxes + * `g`: show/hide parts of the GUI the side columns + * `esc`: quit the simulator + """ + + def __init__(self, robot, interface=None, camera=None, first_person_view=False, speed=10, priority=None, + verbose=False): + """ + Initialize the Bridge between a Mouse-Keyboard interface and a differential wheeled robot instance. + + Args: + robot (AckermannWheeledRobot): wheeled robot instance. + interface (None, MouseKeyboardInterface): mouse keyboard interface. If None, it will create one. + camera (WorldCamera): world camera instance. This will allow the user to switch between first-person and + third-person view. If None, it will create an instance of it. + first_person_view (bool): if True, it will set the world camera to the first person view. If False, it + will be the third-person view. + speed (float): speed of the propeller + priority (int): priority of the bridge. + verbose (bool): If True, print information on the standard output. + """ + if not isinstance(robot, DifferentialWheeledRobot): + raise TypeError("Expecting the given 'robot' to be an instance of `DifferentialWheeledRobot`, instead " + "got: {}".format(type(robot))) + super(BridgeMouseKeyboardDifferentialWheeledRobot, self).__init__(robot, interface=interface, camera=camera, + first_person_view=first_person_view, + speed=speed, priority=priority, + verbose=verbose) + + 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) + elif key.bottom_arrow in down: + self.robot.drive(speed=-self.speed) + elif key.left_arrow in down: + self.robot.turn(speed=self.speed) + elif key.right_arrow in down: + self.robot.turn(speed=-self.speed) + else: + self.robot.drive(speed=0) + + +class BridgeMouseKeyboardAckermannWheeledRobot(BridgeMouseKeyboardWheeledRobot): + r"""Bridge between the mouse-keyboard and a Ackermann wheeled robot. + + Keyboard: + * `top arrow`: move forward + * `bottom arrow`: move backward + * `left arrow`: steer to the left + * `right arrow`: steer to the right + * `space`: switch between first-person and third-person view. + * predefined in simulator: + * `w`: show the wireframe (collision shapes) + * `s`: show the reference system + * `v`: show bounding boxes + * `g`: show/hide parts of the GUI the side columns + * `esc`: quit the simulator + """ + + def __init__(self, robot, interface=None, camera=None, first_person_view=False, speed=10, priority=None, + verbose=False): + """ + Initialize the Bridge between a Mouse-Keyboard interface and a wheeled robot instance. + + Args: + robot (DifferentialWheeledRobot): wheeled robot instance. + interface (None, MouseKeyboardInterface): mouse keyboard interface. If None, it will create one. + camera (WorldCamera): world camera instance. This will allow the user to switch between first-person and + third-person view. If None, it will create an instance of it. + first_person_view (bool): if True, it will set the world camera to the first person view. If False, it + will be the third-person view. + speed (float): speed of the propeller + priority (int): priority of the bridge. + verbose (bool): If True, print information on the standard output. + """ + if not isinstance(robot, AckermannWheeledRobot): + raise TypeError("Expecting the given 'robot' to be an instance of `AckermannWheeledRobot`, instead got: " + "{}".format(type(robot))) + super(BridgeMouseKeyboardAckermannWheeledRobot, self).__init__(robot, interface=interface, camera=camera, + first_person_view=first_person_view, + speed=speed, priority=priority, verbose=verbose) + + 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) + elif key.bottom_arrow in down: + self.robot.drive(speed=self.speed) + elif key.left_arrow in down: + self.robot.steer(speed=self.speed) + elif key.right_arrow in down: + self.robot.steer(speed=-self.speed) + else: + self.robot.drive(speed=0) + + +# Tests +if __name__ == '__main__': + from itertools import count + import pyrobolearn as prl + + # create simulator + sim = prl.simulators.Bullet() + + # create World + world = prl.worlds.BasicWorld(sim) + + # load robot + # robot = world.load_robot('epuck') + robot = prl.robots.Epuck(sim, position=[0, 0, 1.]) + + # create bridge/interface + bridge = BridgeMouseKeyboardDifferentialWheeledRobot(robot, verbose=True) + + for _ in count(): + bridge.step(update_interface=True) + world.step(sleep_dt=sim.dt) diff --git a/pyrobolearn/tools/interfaces/controllers/playstation.py b/pyrobolearn/tools/interfaces/controllers/playstation.py index 25cda1c..d9d8759 100755 --- a/pyrobolearn/tools/interfaces/controllers/playstation.py +++ b/pyrobolearn/tools/interfaces/controllers/playstation.py @@ -4,6 +4,8 @@ This provides the interfaces for the PlayStation controllers (PS3 and PS4) using the `inputs` library. """ +import numpy as np + try: from inputs import devices, get_gamepad except ImportError as e: @@ -53,21 +55,23 @@ class PSControllerInterface(GameControllerInterface): raise ValueError("The specified gamepad/controller was not detected.") # translation - buttons = ['BTN_SOUTH', 'BTN_EAST', 'BTN_WEST', 'BTN_NORTH', 'BTN_THUMBL', 'BTN_THUMBR', 'BTN_TL', 'BTN_TL2', - 'BTN_TR', 'BTN_TR2', 'BTN_START', 'BTN_SELECT', 'BTN_MODE', 'ABS_HAT0X', 'ABS_HAT0Y', 'ABS_Z', - 'ABS_RZ', 'ABS_X', 'ABS_Y', 'ABS_RX', 'ABS_RY'] - ps4_buttons = ['X', 'O', 'S', 'T', 'LJB', 'RJB', 'L1', 'L2', 'R1', 'R2', 'options', 'share', 'PS', 'L', 'R', - 'RT', 'LJX', 'LJY', 'RJX', 'RJY'] + buttons = ['BTN_EAST', 'BTN_C', 'BTN_SOUTH', 'BTN_NORTH', 'BTN_SELECT', 'BTN_START', 'BTN_WEST', 'BTN_TL', + 'BTN_Z', 'BTN_TR', 'BTN_TR2', 'BTN_TL2', 'BTN_MODE', 'BTN_THUMBL', 'ABS_HAT0X', 'ABS_HAT0Y', + 'ABS_X', 'ABS_Y', 'ABS_Z', 'ABS_RZ'] # , 'ABS_RX', 'ABS_RY', 'BTN_THUMBR'] + ps4_buttons = ['X', 'O', 'S', 'T', 'LJB', 'RJB', 'L1', 'L2', 'R1', 'R2', 'options', 'share', 'PS', 'pad', + 'DpadX', 'DpadY', 'LJX', 'LJY', 'RJX', 'RJY'] self.map = dict(zip(buttons, ps4_buttons)) self.inv_map = dict(zip(ps4_buttons, buttons)) # buttons and their values - self.buttons = dict(zip(ps4_buttons[:12], [0] * 12)) - self.buttons.update(dict(zip(['Dpad', 'LJ', 'RJ'], [[0, 0]] * 3))) + self.buttons = dict(zip(ps4_buttons[:14], [0] * 14)) + self.buttons.update(dict(zip(['Dpad', 'LJ', 'RJ'], [np.array([0., 0.])] * 3))) # last updated button self.last_updated_button = None + # pushed buttons + super(PSControllerInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose) ############## @@ -79,24 +83,33 @@ class PSControllerInterface(GameControllerInterface): """Button X""" return self.buttons['X'] + # alias + cross = X + @property def O(self): """Button O (circle)""" return self.buttons['O'] - # alias (C = circle) - C = O + # alias + circle = O @property def S(self): """Button Square""" return self.buttons['S'] + # alias + square = S + @property def T(self): """Button Triangle""" return self.buttons['T'] + # alias + triangle = T + @property def LJB(self): """Left Joystick Button""" @@ -108,34 +121,44 @@ class PSControllerInterface(GameControllerInterface): return self.buttons['RJB'] @property - def LB(self): + def L1(self): """left bumper; button for left index finger""" - return self.buttons['LB'] + return self.buttons['L1'] @property - def RB(self): + def R1(self): """right bumper; button for right index finger""" - return self.buttons['RB'] + return self.buttons['R1'] @property - def menu(self): - """menu button""" - return self.buttons['menu'] - - @property - def view(self): - """view button""" - return self.buttons['view'] - - @property - def LT(self): + def L2(self): """Left trigger; button for left middle finger""" - return self.buttons['LT'] + return self.buttons['L2'] @property - def RT(self): + def R2(self): """Right trigger; button for right middle finger""" - return self.buttons['RT'] + return self.buttons['R2'] + + @property + def options(self): + """menu button""" + return self.buttons['options'] + + @property + def share(self): + """share button""" + return self.buttons['share'] + + @property + def PS(self): + """PS button""" + return self.buttons['PS'] + + @property + def pad(self): + """pad button""" + return self.buttons['pad'] @property def Dpad(self): @@ -156,6 +179,117 @@ class PSControllerInterface(GameControllerInterface): left_joystick = LJ right_joystick = RJ + # NOTE: the following buttons have been manually remapped to better correspond to what their name suggests + @property + def BTN_SOUTH(self): + """South button""" + return self.buttons['X'] + + @property + def BTN_EAST(self): + """East button""" + return self.buttons['O'] + + @property + def BTN_WEST(self): + """West button""" + return self.buttons['S'] + + @property + def BTN_NORTH(self): + """North button""" + return self.buttons['T'] + + @property + def BTN_C(self): + """Circle button""" + return self.buttons['O'] + + @property + def BTN_THUMBL(self): + """Left thumb button""" + return self.buttons['LJB'] + + @property + def BTN_THUMBR(self): + """Right thumb button""" + return self.buttons['RJB'] + + @property + def BTN_TL(self): + """Left bumper; button for left index finger""" + return self.buttons['L1'] + + @property + def BTN_TL2(self): + """Left bumper 2; button for left middle finger""" + return self.buttons['L2'] + + @property + def BTN_TR(self): + """Right bumper; button for right index finger""" + return self.buttons['R1'] + + @property + def BTN_TR2(self): + """Right bumper 2; button for right middle finger""" + return self.buttons['R2'] + + @property + def BTN_START(self): + """Start button""" + return self.buttons['share'] + + @property + def BTN_SELECT(self): + """Select button""" + return self.buttons['options'] + + @property + def BTN_MODE(self): + """Mode button""" + return self.buttons['PS'] + + @property + def ABS_Z(self): + """Left trigger; non-existent for PS controller; return the same as L2.""" + return self.buttons['L2'] + + @property + def ABS_RZ(self): + """Right trigger; non-existent for PS controller; return the same as R2.""" + return self.buttons['R2'] + + @property + def ABS_HAT0X(self): + """Directional pad X position""" + return self.buttons['Dpad'][0] + + @property + def ABS_HAT0Y(self): + """Directional pad Y position""" + return self.buttons['Dpad'][1] + + @property + def ABS_X(self): + """Left joystick X position""" + return self.buttons['LJ'][0] + + @property + def ABS_Y(self): + """Left joystick Y position""" + return self.buttons['LJ'][1] + + @property + def ABS_RX(self): + """Right joystick X position""" + return self.buttons['RJ'][0] + + @property + def ABS_RY(self): + """Right joystick Y position""" + return self.buttons['RJ'][1] + ########### # Methods # ########### @@ -202,16 +336,16 @@ class PSControllerInterface(GameControllerInterface): if event_type == 'Absolute': if key == 'LJX': - self.buttons['LJ'][0] = value / 32768. # values between [-32768, 32767] + self.buttons['LJ'][0] = (value - 127.5) / 127.5 # values between [0, 255] self.last_updated_button = 'LJ' elif key == 'LJY': - self.buttons['LJ'][1] = -1. * value / 32768. # values between [-32767, 32768] + self.buttons['LJ'][1] = -1. * (value - 127.5) / 127.5 # values between [0, 255] self.last_updated_button = 'LJ' elif key == 'RJX': - self.buttons['RJ'][0] = value / 32768. # values between [-32768, 32767] + self.buttons['RJ'][0] = (value - 127.5) / 127.5 # values between [0, 255] self.last_updated_button = 'RJ' elif key == 'RJY': - self.buttons['RJ'][1] = -1. * value / 32768. # values between [-32767, 32768] + self.buttons['RJ'][1] = -1. * (value - 127.5) / 127.5 # values between [0, 255] self.last_updated_button = 'RJ' elif key == 'DpadX': self.buttons['Dpad'][0] = value # left (-1) and right (1) @@ -219,11 +353,11 @@ class PSControllerInterface(GameControllerInterface): elif key == 'DpadY': self.buttons['Dpad'][1] = -1 * value # down (-1) and high (1) self.last_updated_button = 'Dpad' - elif key == 'LT' or key == 'RT': # max 1023 - self.buttons[key] = value / 1023. - # self.last_updated_button = key + # elif key == 'LT' or key == 'RT': # max 1023 + # self.buttons[key] = value / 1023. + # # self.last_updated_button = key elif event_type == 'Key': - print(event_type, key, value) + # print(event_type, key, value) self.buttons[key] = value self.last_updated_button = key @@ -267,12 +401,41 @@ class PS4ControllerInterface(PSControllerInterface): # Tests if __name__ == '__main__': - device = devices.gamepads[1] - print(device.name) + # create controller + controller = PSControllerInterface(use_thread=True, sleep_dt=0.01) + print(controller.map) + print(controller.buttons) + + # check buttons while True: - events = device.read() # blocking=False) # get_gamepad() - for event in events: - event_type, code, state = event.ev_type, event.code, event.state - if event_type != 'Absolute': - if code != 'SYN_REPORT': - print(code, state) + # controller.step() + if controller.cross: + print("X button has been pushed.") + if controller.square: + print("Square button has been pushed.") + if controller.circle: + print("Circle button has been pushed.") + if controller.triangle: + print("Triangle button has been pushed.") + if controller.L1: + print("L1 has been pushed.") + if controller.L2: + print("L2 has been pushed.") + if controller.R1: + print("R1 has been pushed.") + if controller.R2: + print("R2 has been pushed.") + if controller.LJB: + print("LJB has been pushed.") + if controller.RJB: + print("RJB has been pushed.") + if controller.options: + print("Options has been pushed.") + if controller.share: + print("Share has been pushed.") + if controller.PS: + print("PS has been pushed.") + if controller.pad: + print("Pad has been pushed.") + + print("Left joystick: {}".format(controller.LJ[0])) diff --git a/pyrobolearn/tools/interfaces/controllers/xbox.py b/pyrobolearn/tools/interfaces/controllers/xbox.py index 0edce40..8ca648d 100644 --- a/pyrobolearn/tools/interfaces/controllers/xbox.py +++ b/pyrobolearn/tools/interfaces/controllers/xbox.py @@ -18,6 +18,8 @@ References: [1] https://askubuntu.com/questions/783587/how-do-i-get-an-xbox-one-controller-to-work-with-16-04-not-steam """ +import numpy as np + try: from inputs import devices # TODO: update the library inputs to make it non-blocking @@ -71,7 +73,7 @@ class XboxControllerInterface(GameControllerInterface): [3] https://askubuntu.com/questions/783587/how-do-i-get-an-xbox-one-controller-to-work-with-16-04-not-steam """ - def __init__(self, use_thread=False, sleep_dt=0, verbose=False, controller_name='X-Box One'): + def __init__(self, use_thread=False, sleep_dt=0, verbose=False, controller_name='X-Box'): # Check if some gamepads are connected to the computer gamepads = devices.gamepads if len(gamepads) == 0: @@ -100,7 +102,7 @@ class XboxControllerInterface(GameControllerInterface): # buttons and their values self.buttons = dict(zip(xbox_buttons[:12], [0]*12)) - self.buttons.update(dict(zip(['Dpad', 'LJ', 'RJ'], [[0,0]]*3))) + self.buttons.update(dict(zip(['Dpad', 'LJ', 'RJ'], [np.array([0., 0.])]*3))) # last updated button self.last_updated_button = None @@ -143,12 +145,12 @@ class XboxControllerInterface(GameControllerInterface): @property def LB(self): - """left bumper; button for left index finger""" + """Left bumper; button for left index finger""" return self.buttons['LB'] @property def RB(self): - """right bumper; button for right index finger""" + """Right bumper; button for right index finger""" return self.buttons['RB'] @property @@ -190,6 +192,116 @@ class XboxControllerInterface(GameControllerInterface): left_joystick = LJ right_joystick = RJ + @property + def BTN_SOUTH(self): + """South button""" + return self.buttons[self.map['BTN_SOUTH']] + + @property + def BTN_EAST(self): + """East button""" + return self.buttons[self.map['BTN_EAST']] + + @property + def BTN_WEST(self): + """West button""" + return self.buttons[self.map['BTN_WEST']] + + @property + def BTN_NORTH(self): + """North button""" + return self.buttons[self.map['BTN_NORTH']] + + @property + def BTN_C(self): + """Circle button; non-existent for Xbox controller; return the same as the B button.""" + return self.buttons['B'] + + @property + def BTN_THUMBL(self): + """Left thumb button""" + return self.buttons[self.map['BTN_THUMBL']] + + @property + def BTN_THUMBR(self): + """Right thumb button""" + return self.buttons[self.map['BTN_THUMBR']] + + @property + def BTN_TL(self): + """Left bumper; button for left index finger""" + return self.buttons[self.map['BTN_TL']] + + @property + def BTN_TL2(self): + """Left bumper 2; non-existent for Xbox controller; return the same as ABS_Z.""" + return self.ABS_Z + + @property + def BTN_TR(self): + """Right bumper; button for right index finger""" + return self.buttons[self.map['BTN_TR']] + + @property + def BTN_TR2(self): + """Right bumper 2; non-existent for Xbox controller; return the same as ABS_RZ.""" + return self.ABS_RZ + + @property + def BTN_START(self): + """Start button""" + return self.buttons[self.map['BTN_START']] + + @property + def BTN_SELECT(self): + """Select button""" + return self.buttons[self.map['BTN_SELECT']] + + @property + def BTN_MODE(self): + """Mode button; non-existent for Xbox controller; return the same as BTN_SELECT.""" + return self.BTN_SELECT + + @property + def ABS_Z(self): + """Left trigger; button for left middle finger""" + return self.buttons['LT'] + + @property + def ABS_RZ(self): + """Right trigger; button for right middle finger""" + return self.buttons['RT'] + + @property + def ABS_HAT0X(self): + """Directional pad X position""" + return self.buttons['Dpad'][0] + + @property + def ABS_HAT0Y(self): + """Directional pad Y position""" + return self.buttons['Dpad'][1] + + @property + def ABS_X(self): + """Left joystick X position""" + return self.buttons['LJ'][0] + + @property + def ABS_Y(self): + """Left joystick Y position""" + return self.buttons['LJ'][1] + + @property + def ABS_RX(self): + """Right joystick X position""" + return self.buttons['RJ'][0] + + @property + def ABS_RY(self): + """Right joystick Y position""" + return self.buttons['RJ'][1] + ########### # Methods # ########### diff --git a/pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py b/pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py index 05c2cb5..0913924 100644 --- a/pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py +++ b/pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py @@ -71,13 +71,14 @@ class MouseKeyboardInterface(InputInterface): triggered, 4 if it has been released. """ - def __init__(self, simulator, verbose=False): + def __init__(self, simulator=None, verbose=False): """ 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. Args: - simulator (Simulator): simulator instance from which we capture mouse and keyboard events. + simulator (Simulator, None): simulator instance from which we capture mouse and keyboard events. If None, + it will look for the first instantiated simulator. verbose (bool): If True, print information on the standard output. """ super(MouseKeyboardInterface, self).__init__(use_thread=False, sleep_dt=0, verbose=verbose) @@ -113,18 +114,14 @@ class MouseKeyboardInterface(InputInterface): @simulator.setter def simulator(self, simulator): """Set the simulator instance.""" - # if isinstance(simulator, Simulator): - # pass - # elif isinstance(simulator, World): - # simulator = simulator.simulator - # elif isinstance(simulator, Env): - # simulator = simulator.world.simulator - # else: - # if not isinstance(simulator, Simulator): - # raise TypeError("Expecting the simulator to be an instance of Simulator, " - # "got instead {}".format(type(simulator))) - # if isinstance(simulator, World): - # simulator = simulator.simulator + if simulator is None: + if len(Simulator.instances) == 0: + raise RuntimeError("No simulator was given to the `MouseKeyboardInterface`, we thus tried to look " + "for an instantiated simulator but none was found...") + simulator = Simulator.instances[0] # by default, we take the first instantiated simulator + if not isinstance(simulator, Simulator): + raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: " + "{}".format(type(simulator))) self._simulator = simulator @property diff --git a/pyrobolearn/worlds/utils/heightmaps/gpr.py b/pyrobolearn/worlds/utils/heightmaps/gpr.py index e78459c..05c8bbc 100644 --- a/pyrobolearn/worlds/utils/heightmaps/gpr.py +++ b/pyrobolearn/worlds/utils/heightmaps/gpr.py @@ -24,7 +24,7 @@ def gpr_heightmap(init_values, x, y, kernel=None, alpha=1e-10, min_height=0, max Generate a heightmap using gaussian process regression. The advantages of using this method over others to generate terrains lies in the capacity of adding prior knowledge through the kernel and the given initial values. For instance, using a RBF kernel means that we want a smooth terrain instead of a bumpy one. - Furthermore, it allows to generate heightmaps which are not necessary square; i.e. they can be rectangular. + Furthermore, it allows to generate heightmaps which are not necessary square; i.e. they can be rectangular. Warnings: this is pretty difficult to exploit if the given data is not consistent. See `heigthmap_rbf` for a better way to generate heightmap. @@ -65,8 +65,8 @@ def gpr_heightmap(init_values, x, y, kernel=None, alpha=1e-10, min_height=0, max >>> heightmap = gpr_heightmap(init_values, x, y) References: - [1] "Gaussian Processes for Machine Learning", Rasmussen and Williams, 2006 - [2] Sklearn: https://scikit-learn.org/stable/modules/gaussian_process.html + - [1] "Gaussian Processes for Machine Learning", Rasmussen and Williams, 2006 + - [2] Sklearn: https://scikit-learn.org/stable/modules/gaussian_process.html """ # check given x and y if len(x.shape) == 1 and len(y.shape) == 1: diff --git a/pyrobolearn/worlds/world.py b/pyrobolearn/worlds/world.py index da642d4..6a0f22c 100644 --- a/pyrobolearn/worlds/world.py +++ b/pyrobolearn/worlds/world.py @@ -1,6 +1,5 @@ #!/usr/bin/env python -"""Define the the `World` class which allows to specify what constitutes the world (i.e. what elements are in - the world). +"""Define the `World` class which allows to specify what constitutes the world (i.e. what elements are in the world). Dependencies: - `pyrobolearn.simulators` @@ -58,8 +57,8 @@ class World(object): For an excellent overview of available 3D models/scenes, check references [1, 2]. References: - [1] "3D Machine Learning": https://github.com/timzhang642/3D-Machine-Learning - [2] Open3D: http://www.open3d.org/ + - [1] "3D Machine Learning": https://github.com/timzhang642/3D-Machine-Learning + - [2] Open3D: http://www.open3d.org/ """ def __init__(self, simulator, gravity=(0., 0., -9.81)):