diff --git a/examples/README.md b/examples/README.md index edada3d..8684c1a 100644 --- a/examples/README.md +++ b/examples/README.md @@ -2,8 +2,20 @@ In this folder, you will find different examples on how to use the framework. +Warning: this folder is currently being updated; few files might still have some bugs or not +implemented completely. Some other folders will be added in the upcoming days. + You can check the following folders: +- `worlds`: how to create a world in the simulator, load various objects inside and interact with +them, use the camera, and load or generate terrains. +- `robots`: check how to load a specific robot (biped, quadruped, wheeled, etc) into the world. +- `interfaces`: the various interfaces (game controllers, webcam, etc) and bridges that you can use. +- `kinematics`: check how to use forward and inverse kinematics as well as position and velocity control. +- `manipulability`: check how to use the velocity and dynamic manipulability ellipsoids. + +- `states`: how to query the states / observations. +- `models`: the different learning models that you can use. + +- `imitation`: how to use imitation learning with the framework. - `gym/cartpole`: policies are trained with different algorithms on the gym Cartpole environment. -- `robots`: check how to load a specific robot into the world. -- `states`: how to query the states / observations. diff --git a/examples/interfaces/README.md b/examples/interfaces/README.md index 0a26b1a..5ca8360 100644 --- a/examples/interfaces/README.md +++ b/examples/interfaces/README.md @@ -1,4 +1,11 @@ -## Interfaces +## Interfaces and Bridges In this folder, you will find examples on what interfaces you can use and on how you can collect the data from them. -You will also be able to connect an interface with an element of the world (in this case, a robot) using bridges, and see that different bridges can lead to different behaviors while getting the data from the same interface. +You will also be able to connect an interface with an element of the world (in this case, a robot) using bridges, +and see that different bridges can lead to different behaviors while getting the data from the same interface. + +Here are few examples that depict the various interfaces: +1. `mouse_keyboard.py`: use the mouse keyboard interface +2. `webcam.py`: use the webcam interface +3. `playstation.py`: use the Playstation joystick controller interface +4. `xbox.py`: use the Xbox joystick controller interface diff --git a/examples/interfaces/mouse_keyboard.py b/examples/interfaces/mouse_keyboard.py new file mode 100644 index 0000000..e5e3704 --- /dev/null +++ b/examples/interfaces/mouse_keyboard.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python +"""Load the mouse keyboard interface. +""" + +from itertools import count + +import pyrobolearn as prl +from pyrobolearn.tools.interfaces.mouse_keyboard import MouseKeyboardInterface + + +# create simulator +sim = prl.simulators.Bullet() + +# create mouse keyboard interface +interface = MouseKeyboardInterface(sim) + +# run interface +for _ in count(): + # perform a step with the interface + interface.step() + + # print pressed keys + if len(interface.key_down) > 0: + print("Keys that are pressed: {}".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 new file mode 100644 index 0000000..c136aa7 --- /dev/null +++ b/examples/interfaces/playstation.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python +"""Load the Playstation game controller interface +""" + +import time +from itertools import count +import argparse + +from pyrobolearn.tools.interfaces.controllers.playstation import PS3ControllerInterface, PS4ControllerInterface + +# 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') +args = parser.parse_args() + + +# load corresponding Playstation controller interface +if args.controller == 'ps3': + controller = PS3ControllerInterface(verbose=True) +elif args.controller == 'ps4': + controller = PS4ControllerInterface(verbose=False) +else: + raise NotImplementedError("Unknown game controller") + + +# run controller +print('Running controller...') +for _ in count(): + + # run one step with the interface + controller.run() # 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])) + + # sleep a bit + time.sleep(0.01) diff --git a/examples/interfaces/xbox.py b/examples/interfaces/xbox.py new file mode 100644 index 0000000..f32d3a5 --- /dev/null +++ b/examples/interfaces/xbox.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python +"""Load the Xbox game controller interface +""" + +import time +from itertools import count +import argparse + +from pyrobolearn.tools.interfaces.controllers.xbox import Xbox360ControllerInterface, XboxOneControllerInterface + +# 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') +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) +else: + raise NotImplementedError("Unknown game controller") + + +# run controller +print('Running controller...') +for _ in count(): + + # run one step with the interface + controller.run() # same as `step()` if we are not using threads + + # get the last update and print it + 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/manipulability/2d_manipulability.py b/examples/manipulability/2d_manipulability.py new file mode 100644 index 0000000..85350d8 --- /dev/null +++ b/examples/manipulability/2d_manipulability.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python +"""Draw the 2D velocity and force manipulability ellipsoids on the end-effector of a 3-link planar manipulator. + +References: + [1] "Robotics: Modelling, Planning and Control" (section 3.9), Siciliano et al., 2010 +""" + +import time +# from itertools import count +import numpy as np + +import pyrobolearn as prl + + +# create simulator +sim = prl.simulators.Bullet() + +# create world +world = prl.worlds.BasicWorld(sim) + +# create robot +robot = world.load_robot('manipulator2d') +robot.reset_joint_states(q=[0.64453457, -1.65045902, -0.31141744]) + +# change camera view +world.camera.reset(distance=2, yaw=-np.pi / 2, pitch=-np.pi/2.01) + +# draw 2d velocity manipulability ellipsoid +# print(robot.end_effector_names) +end_effector_id = robot.get_link_ids('gripper') +jacobian = robot.get_linear_jacobian(link_id=end_effector_id) +jjt = robot.get_JJT(jacobian) +robot.draw_velocity_manipulability_ellipsoid(link_id=end_effector_id, JJT=jjt, color=(0, 1, 0, 0.7)) # green +robot.draw_force_manipulability_ellipsoid(link_id=end_effector_id, JJT=jjt, color=(1, 0, 0, 0.7)) # red + +# TODO: fix bug + +time.sleep(10000) +# run simulator +# for t in count(): +# world.step(sleep_dt=1./240) diff --git a/examples/manipulability/README.md b/examples/manipulability/README.md index 3c9e9a8..8f2b826 100644 --- a/examples/manipulability/README.md +++ b/examples/manipulability/README.md @@ -2,6 +2,16 @@ We provide examples on how to use manipulability ellipsoids. +Here are a short description of the various examples the user can try: +1. `2d_manipulability.py`: draw the 2D velocity and force manipulability ellipsoids on the end-effector of a +3-link planar manipulator. +2. `com_manipulability_tracking.py`: track the velocity manipulability ellipsoid of the center of mass of a robot +with a fixed base. +3. `com_manipulability_tracking_with_balance.py`: track the velocity manipulability ellipsoid of the center of mass +of a floating-base robot while keeping its balance. +4. `com_dynamic_manipulability_tracking_with_balance.py`: track the dynamic manipulability ellipsoid of the center +of mass of a floating-base robot while keeping its balance. + References: - [1] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010 - [2] "Springer Handbook of Robotics", Siciliano et al., 2008 diff --git a/examples/robots/README.md b/examples/robots/README.md index 3a46ca9..e945c60 100644 --- a/examples/robots/README.md +++ b/examples/robots/README.md @@ -1,5 +1,22 @@ ## Robot examples -You can try to load different robot by typing `python .py`. +More than 60 robots (of various types) are available through `pyrobolearn`. -To turn the camera in the simulator, keep pressing the `ctrl` key and the left button on the mouse, and move this last one. +Here are the few examples that you can find in this folder: +1. `load_robot.py `: load the given robot in the world. +2. `visualize_robot.py `: test different visualization tools that can be used on the robot to show its +joint axis, bounding boxes, and others. +3. `robot_with_sliders.py `: load the given robot in the world and allow you to manipulate the robot's +joints with sliders. +4. `distribute_epucks.py`: distribute several e-pucks in the world and make them move forward. +5. `quadcopter_controller.py`: move a quadcopter in the air using an Xbox or Playstation game controller. +6. `robots/.py`: load the given robot in the simulator by directly instantiating it. Some of these files do +more than just loading the robot. + +Notes: to turn the camera in the simulator, keep pressing the `ctrl` key and the left button on the mouse, and +move this last one. + + +#### What to check next? + +Check the `pyrobolearn/examples/interfaces` or `pyrobolearn/examples/kinematics` folder. diff --git a/examples/robots/distribute_epucks.py b/examples/robots/distribute_epucks.py new file mode 100644 index 0000000..3519785 --- /dev/null +++ b/examples/robots/distribute_epucks.py @@ -0,0 +1,60 @@ +#!/usr/bin/env python +"""Distribute several e-pucks in the world and make them move forward. + +You can move in the world using the keyboard and mouse: +- `ctrl + left click`: rotate the camera +- `scroll wheel` or `ctrl + right click`: zoom in/out +- `ctrl + middle click`: move the camera +- `left click` on an object: if the object has a mass and a collision shape, you can interact with it with the mouse +- `w`: wireframe (see collision shapes) +- `g`: show/hide menu +- `esc`: quit the simulator +""" + +import numpy as np +from itertools import count +import argparse + +import pyrobolearn as prl + + +# create function for the parser to check the number of robots +def check(number): + """check that the number of robots is between 1 and 100.""" + number = int(number) + if number < 1: + number = 1 + if number > 100: + number = 100 + return number + + +# create parser to select the robot +parser = argparse.ArgumentParser() +parser.add_argument('-n', '--number', help='the number of epucks in the world', type=check, default=10) +args = parser.parse_args() + + +# create simulator +sim = prl.simulators.Bullet() + +# create basic world (with a floor and gravity enabled by default) +world = prl.worlds.BasicWorld(sim, scaling=1) + +# specify distribution ranges for position (x,y,z) and orientation (r,p,y) +low_position, high_position = [-3, -3, 0], [3, 3, 0] # x,y,z +low_orientation, high_orientation = [0, 0, -np.pi], [0, 0, np.pi] # r,p,y + +# distribute the epucks in the world +robots = world.distribute(world.load_robot, size=args.number, position_range=(low_position, high_position), + rpy_range=(low_orientation, high_orientation), return_body=True, robot='epuck') + +# run simulator +for t in count(): + + # move each robot forward + for robot in robots: + robot.drive(speed=5) + + # perform one step in the world + world.step(sleep_dt=1. / 240) diff --git a/examples/robots/load_robot.py b/examples/robots/load_robot.py index d13c838..125df9e 100644 --- a/examples/robots/load_robot.py +++ b/examples/robots/load_robot.py @@ -1,33 +1,44 @@ -# This file creates a basic world, load each robot that can be found in the PRL framework +#!/usr/bin/env python +"""Load a robot in a basic world. -from pyrobolearn.simulators import BulletSim -from pyrobolearn.worlds import BasicWorld -from pyrobolearn.robots import implemented_robots +You can move in the world using the keyboard and mouse: +- `ctrl + left click`: rotate the camera +- `scroll wheel` or `ctrl + right click`: zoom in/out +- `ctrl + middle click`: move the camera +- `left click` on an object: if the object has a mass and a collision shape, you can interact with it with the mouse +- `w`: wireframe (see collision shapes) +- `g`: show/hide menu +- `esc`: quit the simulator +""" -robot_not_working = set(['icub']) +from itertools import count +import argparse + +import pyrobolearn as prl + + +# get implemented robots +robots = prl.robots.implemented_robots +print("All the robots (total number of robots = {}): {}".format(len(robots), robots)) + +# create parser to select the robot +parser = argparse.ArgumentParser() +parser.add_argument('-r', '--robot', help='the robot to load in the world', type=str, + choices=robots, default='hyq2max') +args = parser.parse_args() -print("All the robots (total number of robots = {}): {}".format(len(implemented_robots), implemented_robots)) # create simulator -sim = BulletSim() +sim = prl.simulators.Bullet() # create basic world with floor and gravity -world = BasicWorld(sim) +world = prl.worlds.BasicWorld(sim) -# create one robot at a time -for i, robot_name in enumerate(implemented_robots): - if robot_name not in robot_not_working: - # instantiate the given robot - robot = world.load_robot(robot_name) +# load the robot in the world (note that you can create the robot outside the world (not recommended), +# and then give it to the `world.load_robot` method to let know the world that a robot was loaded) +robot = world.load_robot(robot=args.robot, position=[0., 0.]) - # print info about the robot - print("Robot n{}: {}".format(i+1, robot)) - # robot.print_info() - - # run for few moments in the world - for t in range(250): - # run one step and sleep a bit - world.step(sleep_dt=1./240) - - # remove the robot from the world - world.remove(robot) +# run simulator +for _ in count(): + # perform one step in the world + world.step(sleep_dt=1. / 240) diff --git a/examples/robots/quadcopter_controller.py b/examples/robots/quadcopter_controller.py new file mode 100644 index 0000000..5cece42 --- /dev/null +++ b/examples/robots/quadcopter_controller.py @@ -0,0 +1,58 @@ +#!/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 xbox # to use Xbox game controller +$ python quadcopter_controller.py --controller ps3 # to use PS3 game controller +``` +""" + +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 game controller to use', type=str, + choices=['xbox', 'ps3'], default='ps3') +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") + + +# 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 +# controller = 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 one step in the world + world.step(sleep_dt=1. / 240) diff --git a/examples/robots/robot_with_sliders.py b/examples/robots/robot_with_sliders.py new file mode 100644 index 0000000..9016de8 --- /dev/null +++ b/examples/robots/robot_with_sliders.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python +"""Manipulate the robot's joints with sliders. + +You can move in the world using the keyboard and mouse: +- `ctrl + left click`: rotate the camera +- `scroll wheel` or `ctrl + right click`: zoom in/out +- `ctrl + middle click`: move the camera +- `left click` on an object: if the object has a mass and a collision shape, you can interact with it with the mouse +- `w`: wireframe (see collision shapes) +- `g`: show/hide menu +- `esc`: quit the simulator +""" + +from itertools import count +import argparse + +import pyrobolearn as prl + + +# create parser to select the robot +parser = argparse.ArgumentParser() +parser.add_argument('-r', '--robot', help='the robot to load in the world', type=str, + choices=prl.robots.implemented_robots, default='coman') +parser.add_argument('-f', '--fixed_base', help='if we should fix the base when the robot has initially a floating ' + 'base', type=bool, default=True) +args = parser.parse_args() + + +# create simulator +sim = prl.simulators.Bullet() + +# create basic world with floor and gravity +world = prl.worlds.BasicWorld(sim) + +# load the robot in the world +robot = world.load_robot(robot=args.robot, position=[0., 0.], fixed_base=args.fixed_base) + +# add a slider for each specified joint +robot.add_joint_slider(joint_ids=robot.joints) + +# run simulator +for _ in count(): + # update the joint slider + robot.update_joint_slider() + + # perform one step in the world + world.step(sleep_dt=1. / 240) diff --git a/examples/robots/aibo.py b/examples/robots/robots/aibo.py similarity index 89% rename from examples/robots/aibo.py rename to examples/robots/robots/aibo.py index 33ebdff..6d98797 100644 --- a/examples/robots/aibo.py +++ b/examples/robots/robots/aibo.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Aibo # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/allegrohand.py b/examples/robots/robots/allegrohand.py similarity index 91% rename from examples/robots/allegrohand.py rename to examples/robots/robots/allegrohand.py index c91507e..c617da0 100644 --- a/examples/robots/allegrohand.py +++ b/examples/robots/robots/allegrohand.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import AllegroHand # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/ant.py b/examples/robots/robots/ant.py similarity index 89% rename from examples/robots/ant.py rename to examples/robots/robots/ant.py index 8147102..39b1c84 100644 --- a/examples/robots/ant.py +++ b/examples/robots/robots/ant.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Ant # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/atlas.py b/examples/robots/robots/atlas.py similarity index 88% rename from examples/robots/atlas.py rename to examples/robots/robots/atlas.py index a82c71d..ec7496b 100644 --- a/examples/robots/atlas.py +++ b/examples/robots/robots/atlas.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Atlas # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/ballbot.py b/examples/robots/robots/ballbot.py similarity index 87% rename from examples/robots/ballbot.py rename to examples/robots/robots/ballbot.py index c9baeb3..7c0cdd9 100644 --- a/examples/robots/ballbot.py +++ b/examples/robots/robots/ballbot.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Ballbot # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/baxter.py b/examples/robots/robots/baxter.py similarity index 88% rename from examples/robots/baxter.py rename to examples/robots/robots/baxter.py index 1fedbed..8ed66d7 100644 --- a/examples/robots/baxter.py +++ b/examples/robots/robots/baxter.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Baxter # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/bb8.py b/examples/robots/robots/bb8.py similarity index 87% rename from examples/robots/bb8.py rename to examples/robots/robots/bb8.py index b1b5546..20c72c1 100644 --- a/examples/robots/bb8.py +++ b/examples/robots/robots/bb8.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import BB8 # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/cartpole.py b/examples/robots/robots/cartpole.py similarity index 98% rename from examples/robots/cartpole.py rename to examples/robots/robots/cartpole.py index 423bd20..138a055 100644 --- a/examples/robots/cartpole.py +++ b/examples/robots/robots/cartpole.py @@ -85,12 +85,12 @@ class LQR(object): if __name__ == "__main__": import numpy as np from itertools import count - from pyrobolearn.simulators import BulletSim + from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import World from pyrobolearn.robots import CartPole # Create simulator - sim = BulletSim() + sim = Bullet() # create world world = World(sim) diff --git a/examples/robots/cassie.py b/examples/robots/robots/cassie.py similarity index 89% rename from examples/robots/cassie.py rename to examples/robots/robots/cassie.py index 92c83af..30684fb 100644 --- a/examples/robots/cassie.py +++ b/examples/robots/robots/cassie.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Cassie # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/centauro.py b/examples/robots/robots/centauro.py similarity index 90% rename from examples/robots/centauro.py rename to examples/robots/robots/centauro.py index 2b9024a..ecd56b3 100644 --- a/examples/robots/centauro.py +++ b/examples/robots/robots/centauro.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Centauro # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/cogimon.py b/examples/robots/robots/cogimon.py similarity index 88% rename from examples/robots/cogimon.py rename to examples/robots/robots/cogimon.py index 8761d06..fb63156 100644 --- a/examples/robots/cogimon.py +++ b/examples/robots/robots/cogimon.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Cogimon # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/coman.py b/examples/robots/robots/coman.py similarity index 92% rename from examples/robots/coman.py rename to examples/robots/robots/coman.py index a853b93..9a7a68a 100644 --- a/examples/robots/coman.py +++ b/examples/robots/robots/coman.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Coman # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/crab.py b/examples/robots/robots/crab.py similarity index 89% rename from examples/robots/crab.py rename to examples/robots/robots/crab.py index 88a0add..5da82ac 100644 --- a/examples/robots/crab.py +++ b/examples/robots/robots/crab.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Crab # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/cubli.py b/examples/robots/robots/cubli.py similarity index 94% rename from examples/robots/cubli.py rename to examples/robots/robots/cubli.py index 405f4d3..f67b6a7 100644 --- a/examples/robots/cubli.py +++ b/examples/robots/robots/cubli.py @@ -5,12 +5,12 @@ import numpy as np from itertools import count from pyrobolearn.utils.transformation import get_rpy_from_quaternion -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Cubli # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/darwin.py b/examples/robots/robots/darwin.py similarity index 89% rename from examples/robots/darwin.py rename to examples/robots/robots/darwin.py index af420af..2035b7b 100644 --- a/examples/robots/darwin.py +++ b/examples/robots/robots/darwin.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Darwin # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/edo.py b/examples/robots/robots/edo.py similarity index 87% rename from examples/robots/edo.py rename to examples/robots/robots/edo.py index 6b7b4e9..7f23baa 100644 --- a/examples/robots/edo.py +++ b/examples/robots/robots/edo.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Edo # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/epuck.py b/examples/robots/robots/epuck.py similarity index 91% rename from examples/robots/epuck.py rename to examples/robots/robots/epuck.py index 8ec101d..f93e0fd 100644 --- a/examples/robots/epuck.py +++ b/examples/robots/robots/epuck.py @@ -4,12 +4,12 @@ import numpy as np from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Epuck # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/f10_racecar.py b/examples/robots/robots/f10_racecar.py similarity index 89% rename from examples/robots/f10_racecar.py rename to examples/robots/robots/f10_racecar.py index cf06cf0..bdb079c 100644 --- a/examples/robots/f10_racecar.py +++ b/examples/robots/robots/f10_racecar.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import F10Racecar # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/fetch.py b/examples/robots/robots/fetch.py similarity index 88% rename from examples/robots/fetch.py rename to examples/robots/robots/fetch.py index ab1bbc7..cb07619 100644 --- a/examples/robots/fetch.py +++ b/examples/robots/robots/fetch.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Fetch # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/franka.py b/examples/robots/robots/franka.py similarity index 90% rename from examples/robots/franka.py rename to examples/robots/robots/franka.py index e04f5ff..0585912 100644 --- a/examples/robots/franka.py +++ b/examples/robots/robots/franka.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Franka # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/half_cheetah.py b/examples/robots/robots/half_cheetah.py similarity index 87% rename from examples/robots/half_cheetah.py rename to examples/robots/robots/half_cheetah.py index f99115d..fdcaa86 100644 --- a/examples/robots/half_cheetah.py +++ b/examples/robots/robots/half_cheetah.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import HalfCheetah # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/hopper.py b/examples/robots/robots/hopper.py similarity index 86% rename from examples/robots/hopper.py rename to examples/robots/robots/hopper.py index dd17a83..8ddb43b 100644 --- a/examples/robots/hopper.py +++ b/examples/robots/robots/hopper.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Hopper # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/hubo.py b/examples/robots/robots/hubo.py similarity index 88% rename from examples/robots/hubo.py rename to examples/robots/robots/hubo.py index 1eb633a..51bc465 100644 --- a/examples/robots/hubo.py +++ b/examples/robots/robots/hubo.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Hubo # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/humanoid.py b/examples/robots/robots/humanoid.py similarity index 89% rename from examples/robots/humanoid.py rename to examples/robots/robots/humanoid.py index 889c1a9..aa71159 100644 --- a/examples/robots/humanoid.py +++ b/examples/robots/robots/humanoid.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Humanoid # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/husky.py b/examples/robots/robots/husky.py similarity index 88% rename from examples/robots/husky.py rename to examples/robots/robots/husky.py index 68a1ac8..9e8f4fc 100644 --- a/examples/robots/husky.py +++ b/examples/robots/robots/husky.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Husky # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/hyq.py b/examples/robots/robots/hyq.py similarity index 90% rename from examples/robots/hyq.py rename to examples/robots/robots/hyq.py index 633a9b1..1fcdab7 100644 --- a/examples/robots/hyq.py +++ b/examples/robots/robots/hyq.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import HyQ # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/hyq2max.py b/examples/robots/robots/hyq2max.py similarity index 90% rename from examples/robots/hyq2max.py rename to examples/robots/robots/hyq2max.py index 7ed24fc..2f34c2f 100644 --- a/examples/robots/hyq2max.py +++ b/examples/robots/robots/hyq2max.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import HyQ2Max # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/jaco.py b/examples/robots/robots/jaco.py similarity index 86% rename from examples/robots/jaco.py rename to examples/robots/robots/jaco.py index 6d86cea..2f2a463 100644 --- a/examples/robots/jaco.py +++ b/examples/robots/robots/jaco.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Jaco # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/kr5.py b/examples/robots/robots/kr5.py similarity index 88% rename from examples/robots/kr5.py rename to examples/robots/robots/kr5.py index 94d39f4..afbf260 100644 --- a/examples/robots/kr5.py +++ b/examples/robots/robots/kr5.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import KR5 # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/kuka_iiwa.py b/examples/robots/robots/kuka_iiwa.py similarity index 95% rename from examples/robots/kuka_iiwa.py rename to examples/robots/robots/kuka_iiwa.py index a2582a0..a2296dc 100644 --- a/examples/robots/kuka_iiwa.py +++ b/examples/robots/robots/kuka_iiwa.py @@ -4,12 +4,12 @@ import numpy as np from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import KukaIIWA # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/kuka_lwr.py b/examples/robots/robots/kuka_lwr.py similarity index 88% rename from examples/robots/kuka_lwr.py rename to examples/robots/robots/kuka_lwr.py index 696b77c..62f5e5d 100644 --- a/examples/robots/kuka_lwr.py +++ b/examples/robots/robots/kuka_lwr.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import KukaLWR # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/laikago.py b/examples/robots/robots/laikago.py similarity index 89% rename from examples/robots/laikago.py rename to examples/robots/robots/laikago.py index 12fb442..9edfc10 100644 --- a/examples/robots/laikago.py +++ b/examples/robots/robots/laikago.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Laikago # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/littledog.py b/examples/robots/robots/littledog.py similarity index 90% rename from examples/robots/littledog.py rename to examples/robots/robots/littledog.py index 4767b59..39a87f6 100644 --- a/examples/robots/littledog.py +++ b/examples/robots/robots/littledog.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import LittleDog # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/manipulator2d.py b/examples/robots/robots/manipulator2d.py similarity index 89% rename from examples/robots/manipulator2d.py rename to examples/robots/robots/manipulator2d.py index c881564..2636b67 100644 --- a/examples/robots/manipulator2d.py +++ b/examples/robots/robots/manipulator2d.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Manipulator2D # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/minitaur.py b/examples/robots/robots/minitaur.py similarity index 90% rename from examples/robots/minitaur.py rename to examples/robots/robots/minitaur.py index 11df4f1..1fdf45e 100644 --- a/examples/robots/minitaur.py +++ b/examples/robots/robots/minitaur.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Minitaur # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/mkz.py b/examples/robots/robots/mkz.py similarity index 89% rename from examples/robots/mkz.py rename to examples/robots/robots/mkz.py index 96e6119..6817fa0 100644 --- a/examples/robots/mkz.py +++ b/examples/robots/robots/mkz.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import MKZ # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/morphex.py b/examples/robots/robots/morphex.py similarity index 86% rename from examples/robots/morphex.py rename to examples/robots/robots/morphex.py index 2c3a8cf..06e572a 100644 --- a/examples/robots/morphex.py +++ b/examples/robots/robots/morphex.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Morphex # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/nao.py b/examples/robots/robots/nao.py similarity index 88% rename from examples/robots/nao.py rename to examples/robots/robots/nao.py index 36da221..fd44718 100644 --- a/examples/robots/nao.py +++ b/examples/robots/robots/nao.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Nao # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/opendog.py b/examples/robots/robots/opendog.py similarity index 88% rename from examples/robots/opendog.py rename to examples/robots/robots/opendog.py index 991de4a..6fd94c7 100644 --- a/examples/robots/opendog.py +++ b/examples/robots/robots/opendog.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import OpenDog # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/pepper.py b/examples/robots/robots/pepper.py similarity index 89% rename from examples/robots/pepper.py rename to examples/robots/robots/pepper.py index 2bc433d..b547a7e 100644 --- a/examples/robots/pepper.py +++ b/examples/robots/robots/pepper.py @@ -3,13 +3,13 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Pepper # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/phantomx.py b/examples/robots/robots/phantomx.py similarity index 86% rename from examples/robots/phantomx.py rename to examples/robots/robots/phantomx.py index 4e5ea5e..d12dfc4 100644 --- a/examples/robots/phantomx.py +++ b/examples/robots/robots/phantomx.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import PhantomX # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/pleurobot.py b/examples/robots/robots/pleurobot.py similarity index 90% rename from examples/robots/pleurobot.py rename to examples/robots/robots/pleurobot.py index 16477c5..65acfe1 100644 --- a/examples/robots/pleurobot.py +++ b/examples/robots/robots/pleurobot.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Pleurobot # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/pr2.py b/examples/robots/robots/pr2.py similarity index 88% rename from examples/robots/pr2.py rename to examples/robots/robots/pr2.py index 0ae5c64..93152f8 100644 --- a/examples/robots/pr2.py +++ b/examples/robots/robots/pr2.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import PR2 # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/quadcopter.py b/examples/robots/robots/quadcopter.py similarity index 86% rename from examples/robots/quadcopter.py rename to examples/robots/robots/quadcopter.py index 1e03960..6b6db19 100644 --- a/examples/robots/quadcopter.py +++ b/examples/robots/robots/quadcopter.py @@ -4,14 +4,14 @@ import numpy as np from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Quadcopter from pyrobolearn.utils.units import rpm_to_rad_per_second # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) @@ -24,7 +24,7 @@ robot.print_info() rpm = robot.get_stationary_rpm() print("Stationary RPM: {}".format(rpm)) -v = rpm_to_rad_per_second(rpm+20) +v = rpm_to_rad_per_second(rpm + 20) v = [v, -v, v, -v] # run simulation diff --git a/examples/robots/rhex.py b/examples/robots/robots/rhex.py similarity index 89% rename from examples/robots/rhex.py rename to examples/robots/robots/rhex.py index fb14874..2bb55cb 100644 --- a/examples/robots/rhex.py +++ b/examples/robots/robots/rhex.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Rhex # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/rrbot.py b/examples/robots/robots/rrbot.py similarity index 94% rename from examples/robots/rrbot.py rename to examples/robots/robots/rrbot.py index a0047d3..efeb5d4 100644 --- a/examples/robots/rrbot.py +++ b/examples/robots/robots/rrbot.py @@ -4,12 +4,12 @@ import numpy as np from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import RRBot # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) @@ -103,7 +103,7 @@ J = sim.calculate_jacobian(robot.id, 1, [0., 0., 0.]) print(np.array(J[0])) a = robot.get_joint_positions() -# print(robot.get_jacobian(1, np.array([0.,0.]))) # TODO: need to convert numpy array to list +# print(robot.get_jacobian(1, np.array([0., 0.]))) # TODO: need to convert numpy array to list linkId = 2 com_frame = robot.get_link_states(linkId)[2] @@ -125,14 +125,14 @@ for i in range(10000): dx = np.array(robot.get_link_world_linear_velocities(linkId)) tau = robot.calculate_inverse_dynamics(ddq, dq, q) # Coriolis, centrifugal and gravity compensation Jlin = np.array(sim.calculate_jacobian(robot.id, linkId, com_frame)[0]) - F = K.dot(xdes - x) - D.dot(dx) # compute cartesian forces + F = K.dot(xdes - x) - D.dot(dx) # evaluate cartesian forces # print("force: {}".format(F)) tau += Jlin.T.dot(F) # cartesian PD with gravity compensation # tau += Jlin.T.dot(- D.dot(dx)) # active compliance # tau = Jlin.T.dot(F) - # compute manipulability measure :math:`w = sqrt(det(JJ^T))` + # evaluate manipulability measure :math:`w = sqrt(det(JJ^T))` Jlin = Jlin[[0, 2], :] w = np.sqrt(np.linalg.det(Jlin.dot(Jlin.T))) # print("manipulability: {}".format(w)) diff --git a/examples/robots/sawyer.py b/examples/robots/robots/sawyer.py similarity index 88% rename from examples/robots/sawyer.py rename to examples/robots/robots/sawyer.py index b47e062..a60aa0e 100644 --- a/examples/robots/sawyer.py +++ b/examples/robots/robots/sawyer.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Sawyer # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/sea_hexapod.py b/examples/robots/robots/sea_hexapod.py similarity index 86% rename from examples/robots/sea_hexapod.py rename to examples/robots/robots/sea_hexapod.py index 37ee317..5d59a27 100644 --- a/examples/robots/sea_hexapod.py +++ b/examples/robots/robots/sea_hexapod.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import SEAHexapod # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/sea_snake.py b/examples/robots/robots/sea_snake.py similarity index 86% rename from examples/robots/sea_snake.py rename to examples/robots/robots/sea_snake.py index 042a8df..0b1f5b8 100644 --- a/examples/robots/sea_snake.py +++ b/examples/robots/robots/sea_snake.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import SEASnake # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/softhand.py b/examples/robots/robots/softhand.py similarity index 93% rename from examples/robots/softhand.py rename to examples/robots/robots/softhand.py index 7f8d171..64d5ab1 100644 --- a/examples/robots/softhand.py +++ b/examples/robots/robots/softhand.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import SoftHand # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/swimmer.py b/examples/robots/robots/swimmer.py similarity index 86% rename from examples/robots/swimmer.py rename to examples/robots/robots/swimmer.py index 5668c4a..c85b93c 100644 --- a/examples/robots/swimmer.py +++ b/examples/robots/robots/swimmer.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Swimmer # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/walker2d.py b/examples/robots/robots/walker2d.py similarity index 86% rename from examples/robots/walker2d.py rename to examples/robots/robots/walker2d.py index 95b408b..e8b5afa 100644 --- a/examples/robots/walker2d.py +++ b/examples/robots/robots/walker2d.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Walker2D # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/walkman.py b/examples/robots/robots/walkman.py similarity index 91% rename from examples/robots/walkman.py rename to examples/robots/robots/walkman.py index c05d76c..f98e8d2 100644 --- a/examples/robots/walkman.py +++ b/examples/robots/robots/walkman.py @@ -3,12 +3,12 @@ """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import Walkman # Create simulator -sim = BulletSim() +sim = Bullet() # Create world world = BasicWorld(sim) diff --git a/examples/robots/wam.py b/examples/robots/robots/wam.py similarity index 95% rename from examples/robots/wam.py rename to examples/robots/robots/wam.py index d9787c4..c932802 100644 --- a/examples/robots/wam.py +++ b/examples/robots/robots/wam.py @@ -4,12 +4,12 @@ import numpy as np from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import WAM # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/youbot.py b/examples/robots/robots/youbot.py similarity index 93% rename from examples/robots/youbot.py rename to examples/robots/robots/youbot.py index e8824de..e0dede3 100644 --- a/examples/robots/youbot.py +++ b/examples/robots/robots/youbot.py @@ -5,12 +5,12 @@ These include: YoubotBase, KukaYoubotArm, Youbot, YoubotDualArm """ from itertools import count -from pyrobolearn.simulators import BulletSim +from pyrobolearn.simulators import Bullet from pyrobolearn.worlds import BasicWorld from pyrobolearn.robots import YoubotBase, KukaYoubotArm, Youbot, YoubotDualArm # Create simulator -sim = BulletSim() +sim = Bullet() # create world world = BasicWorld(sim) diff --git a/examples/robots/visualize_robot.py b/examples/robots/visualize_robot.py new file mode 100644 index 0000000..b63138f --- /dev/null +++ b/examples/robots/visualize_robot.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python +"""Test different visualization tools that can be used on the robot. + + +Test different visualization tools on a robot. You can notably: +- render the robot semi-transparent +- draw the robot center of mass and the projected center of mass +- draw the center of mass of each link +- draw the link frames +- draw the joint axis +- draw bounding boxes around links +- for legged robots: + - draw ground reference points such as the ZMP, COP, FRI, and CMP + - draw the support polygon + - draw friction cones +- draw velocity and dynamic manipulability ellipsoids + + +You can move in the world using the keyboard and mouse: +- `ctrl + left click`: rotate the camera +- `scroll wheel` or `ctrl + right click`: zoom in/out +- `ctrl + middle click`: move the camera +- `left click` on an object: if the object has a mass and a collision shape, you can interact with it with the mouse +- `w`: wireframe (see collision shapes) +- `g`: show/hide menu +- `esc`: quit the simulator +""" + +import time +from itertools import count +import argparse + +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') +args = parser.parse_args() + + +# create simulator +sim = prl.simulators.Bullet() + +# create basic world with floor and gravity +world = prl.worlds.BasicWorld(sim) + +# load the robot in the world +robot = world.load_robot(robot=args.robot, position=[0., 0.]) + +# move the simulation a bit forward (such that the robot touches the floor) +for _ in range(100): + world.step() + +# change visualization +robot.change_transparency() +robot.draw_link_coms() +robot.draw_link_frames() +robot.draw_bounding_boxes(link_ids=-1) + +robot.draw_friction_cone(floor_id=world.floor_id) +robot.draw_support_polygon(floor_id=world.floor_id, lifetime=0) + +robot.compute_and_draw_com_position() +robot.compute_and_draw_projected_com_position() +# robot.draw_cop(cop=world.floor_id) +# robot.draw_zmp(zmp=world.floor_id) +# robot.draw_cmp(cmp=world.floor_id) + +time.sleep(10000) +# run simulator +# for t in count(): +# # perform one step in the world +# world.step(sleep_dt=1. / 240) diff --git a/examples/worlds/README.md b/examples/worlds/README.md index 5831848..4fd4b29 100644 --- a/examples/worlds/README.md +++ b/examples/worlds/README.md @@ -14,3 +14,8 @@ and with collisions) that are movable, fixed, or are moving. 4. `load_robot.py`: load a robot in a basic world and distribute randomly few objects on the floor. 5. `load_heightmap.py`: load a terrain from a heightmap (png) and load a robot on it. 6. `generate_terrain.py`: generate a terrain and distribute randomly few objects on the terrain. + + +#### What to check next? + +Check the `pyrobolearn/examples/robots` folder. diff --git a/pyrobolearn/robots/__init__.py b/pyrobolearn/robots/__init__.py index 77cf1c4..07b2c0f 100644 --- a/pyrobolearn/robots/__init__.py +++ b/pyrobolearn/robots/__init__.py @@ -129,6 +129,9 @@ for s in ['__init__', 'actuators', 'sensors', 'legged_robot', 'manipulator', 'wh implemented_robots = list(implemented_robots) +# TODO: fix problem with icub +implemented_robots.remove('icub') + # create dictionary that maps robot names to robot classes robot_names_to_classes = {} diff --git a/pyrobolearn/robots/quadcopter.py b/pyrobolearn/robots/quadcopter.py index 29402b0..cdd0bc8 100644 --- a/pyrobolearn/robots/quadcopter.py +++ b/pyrobolearn/robots/quadcopter.py @@ -7,7 +7,7 @@ import numpy as np 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 +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" @@ -62,6 +62,8 @@ class Quadcopter(RotaryWingUAV): [7] "Propeller Static & Dynamic Thrust Calculation": https://www.electricrcaircraftguy.com/2013/09/propeller-static-dynamic-thrust-equation.html https://www.electricrcaircraftguy.com/2014/04/propeller-static-dynamic-thrust-equation-background.html + [8] "Flying Principle of a Quadrotor" (from the course "Autonomous Navigation for Flying Robots" on EdX), + Jurgen, https://jsturm.de/publications/data/lecture_1_part_3.pdf """ def __init__(self, @@ -96,7 +98,7 @@ class Quadcopter(RotaryWingUAV): # joints 1 and 3 are CCW, and joints 2 and 4 are CW # CCW = +1, CW = -1 - self.turning_directions = [+1, -1, +1, -1] + self.propeller_directions = np.array([+1, -1, +1, -1]) # Propeller pitches are around 0.0762m (3 inches) and 0.127m (5 inches) # (from https://www.dronezon.com/learn-about-drones-quadcopters/how-a-quadcopter-works-with-propellers-and\ @@ -112,6 +114,12 @@ class Quadcopter(RotaryWingUAV): self.k1 = 1./3.29546 self.k2 = 1.5 + # joint velocity to fly on the spot (hovering) + v = self.get_stationary_joint_velocity() + self.stationary_velocities = np.array([v, -v, v, -v]) + + # initially: propellers[0] = +x, propellers[1] = -y, propellers[2] = -x, propellers[3] = +y + def calculate_thrust_force(self, angular_speed, area, propeller_pitch, v0=0, air_density=1.225): r""" Calculate the thrust force generated by the propeller (based on [6]). @@ -159,7 +167,7 @@ class Quadcopter(RotaryWingUAV): super(Quadcopter, self).set_joint_velocities(velocities, joint_ids, forces, max_velocity) # calculate thrust force of the given joints, and apply it on the link - for jnt, d, v in zip(joint_ids, self.turning_directions, velocities): + for jnt, d, v in zip(joint_ids, self.propeller_directions, velocities): if max_velocity and v > self.max_velocity: v = self.max_velocity @@ -188,18 +196,147 @@ class Quadcopter(RotaryWingUAV): p = self.propeller_pitch return (60 / p) * (fg / (self.air_density * self.area) * (p / (self.k1 * self.diameter))**self.k2)**0.5 - # def gravityCompensate(self): + # def gravity_compensate(self): # pass + def hover(self): + """Hover; let the quadcopter fly on the spot.""" + self.set_propeller_velocities(self.stationary_velocities) + + @staticmethod + def rpm_to_rad_per_second(rpm): + """ + Convert the revolutions per minute to rad/sec. + + Args: + rpm (float): revolutions per minute. + + Returns: + float: rad/sec + """ + return rpm_to_rad_per_second(rpm) + + @staticmethod + def rad_per_second_to_rpm(omega): + """ + Convert rad/sec to revolutions/minute. + + Args: + omega (float): angular velocity (rad/sec) + + Returns: + float: revolutions/minute + """ + return rad_per_second_to_rpm(omega) + + def move(self, velocity): + """Move the robot at the specified 3D velocity vector. + + Args: + velocity (np.array[3]): 3D velocity vector defined in the xy plane. The magnitude represents the speed. + """ + speed = np.linalg.norm(velocity[:2]) + angle = np.arctan2(velocity[1], velocity[0]) + angles = np.array([angle]*4) + np.array([0, np.pi/2, np.pi, 3*np.pi/2]) + idx = angles > np.pi + angles[idx] = 2*np.pi - angles[idx] # convert from range [0, 2pi[ to [-pi, pi[ + velocities = np.abs(angles / np.pi) + velocities = self.stationary_velocities + speed * velocities * self.propeller_directions + if len(velocity) > 2: # z direction + velocities += 10 * velocity[2] * self.propeller_directions # 10 is a random constant + self.set_propeller_velocities(velocities) + + def ascend(self, speed=0): + """Make the quadcopter ascend. + + Args: + speed (float): speed to ascend. + """ + self.set_propeller_velocities(self.stationary_velocities + speed * self.propeller_directions) + + def descend(self, speed=0): + """Make the quadcopter descend. + + Args: + speed (float): speed to descend. + """ + self.set_propeller_velocities(self.stationary_velocities - speed * self.propeller_directions) + + def turn(self, speed=0): + """Turn the quadcopter. 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). + """ + if speed > 0: + self.turn_left(np.abs(speed)) + else: + self.turn_right(np.abs(speed)) + + def turn_left(self, speed=0): + """Turn the quadcopter to the left. + + Args: + speed (float): 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) + + def turn_right(self, speed=0): + """Turn the quadcopter to the right. + + Args: + speed (float): 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) + + def move_forward(self, speed=0): + """Move the quadcopter forward. + + Args: + speed (float): speed to move forward. + """ + velocities = self.stationary_velocities + speed * np.array([0., 0.5, 1., 0.5]) * self.propeller_directions + self.set_propeller_velocities(velocities) + + def move_backward(self, speed=0): + """Move the quadcopter backward. + + Args: + speed (float): speed to move backward. + """ + velocities = self.stationary_velocities + speed * np.array([1., 0.5, 0., 0.5]) * self.propeller_directions + self.set_propeller_velocities(velocities) + + def move_left(self, speed=0): + """Tilt the quadcopter to the left. + + Args: + speed (float): speed to move to the left. + """ + velocities = self.stationary_velocities + speed * np.array([0.5, 1., 0.5, 0.]) * self.propeller_directions + self.set_propeller_velocities(velocities) + + def move_right(self, speed=0): + """Tilt the quadcopter to the right. + + Args: + speed (float): speed to move to the right. + """ + velocities = self.stationary_velocities + speed * np.array([0.5, 0., 0.5, 1.]) * self.propeller_directions + self.set_propeller_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) diff --git a/pyrobolearn/robots/robot.py b/pyrobolearn/robots/robot.py index b3cba32..9ed4e32 100644 --- a/pyrobolearn/robots/robot.py +++ b/pyrobolearn/robots/robot.py @@ -2182,7 +2182,7 @@ class Robot(ControllableBody): Me = logarithm_map([target_velocity_manipulability], velocity_manip[0:num_task_vars, 0:num_task_vars])[0] # print("Me: {}".format(Me)) distance = distance_spd(target_velocity_manipulability, velocity_manip[0:num_task_vars, 0:num_task_vars]) - # print("SPD dist: {}".format(distance)) + # print("SPD distance: {}".format(distance)) Jm_red = self.compute_velocity_manipulability_jacobian(jacobian, num_task_vars) # print("Jm: {}".format(Jm_red)) @@ -2899,7 +2899,7 @@ class Robot(ControllableBody): # print("Me: {}".format(Me)) distance = distance_spd(target_dynamic_manipulability[0:num_task_vars, 0:num_task_vars], dynamic_manip[0:num_task_vars, 0:num_task_vars]) - print("SPD dist: {}".format(distance)) + print("SPD distance: {}".format(distance)) Jm_red = self.compute_dynamic_manipulability_jacobian(jacobian, inertia, num_task_vars) # print("Jm: {}".format(Jm_red)) diff --git a/pyrobolearn/robots/uav.py b/pyrobolearn/robots/uav.py index 41ca1da..02187fc 100644 --- a/pyrobolearn/robots/uav.py +++ b/pyrobolearn/robots/uav.py @@ -54,6 +54,39 @@ class RotaryWingUAV(UAVRobot): def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scale=1.): super(RotaryWingUAV, self).__init__(simulator, urdf, position, orientation, fixed_base, scale) + def hover(self): + pass + + def move(self, velocity): + pass + + def ascend(self, speed=0): + pass + + def descend(self, speed=0): + pass + + def turn(self, speed=0): + pass + + def turn_left(self, speed=0): + pass + + def turn_right(self, speed=0): + pass + + def move_forward(self, speed=0): + pass + + def move_backward(self, speed=0): + pass + + def move_left(self, speed=0): + pass + + def move_right(self, speed=0): + pass + class FlappingWingUAV(UAVRobot): r"""Flapping Wing Robot diff --git a/pyrobolearn/simulators/bullet.py b/pyrobolearn/simulators/bullet.py index 77d4780..312a733 100644 --- a/pyrobolearn/simulators/bullet.py +++ b/pyrobolearn/simulators/bullet.py @@ -125,6 +125,10 @@ class Bullet(Simulator): # given parameters self.kwargs = {'render': render, 'kwargs': kwargs} + # define default timestep + self.default_timestep = 1. / 240 + self.dt = self.default_timestep + # go through the global variables / attributes defined in pybullet and set them here # this includes for instance: JOINT_REVOLUTE, POSITION_CONTROL, etc. # for attribute in dir(pybullet): @@ -160,6 +164,11 @@ class Bullet(Simulator): """Return the version of the simulator in a year-month-day format.""" return self.sim.getAPIVersion() + @property + def timestep(self): + """Return the simulator time step.""" + return self.dt + ############# # Operators # ############# @@ -386,6 +395,14 @@ class Bullet(Simulator): # set the render variable (useful when calling the method `is_rendering`) self._render = enable + def get_time_step(self): + """Get the time step in the simulator. + + Returns: + float: time step in the simulator + """ + return self.get_physics_properties()['fixed_time_step'] + def set_time_step(self, time_step): """Set the specified time step in the simulator. @@ -401,6 +418,7 @@ class Bullet(Simulator): time_step (float): Each time you call 'step' the time step will proceed with 'time_step'. """ # self.history.append(('set_time_step', {'time_step': time_step})) + self.dt = time_step self.sim.setTimeStep(timeStep=time_step) def set_real_time(self, enable=True): diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py index 4a37bd6..805f4b0 100644 --- a/pyrobolearn/simulators/simulator.py +++ b/pyrobolearn/simulators/simulator.py @@ -184,6 +184,10 @@ class Simulator(object): # main camera in the simulator self._camera = None + # default timestep + self.default_timestep = 1. / 240 + self.dt = self.default_timestep + # 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): @@ -214,6 +218,11 @@ class Simulator(object): """Return the camera (yaw, pitch, distance, target_position) or None.""" return self._camera + @property + def timestep(self): + """Return the simulator time step.""" + return self.get_time_step() + ############# # Operators # ############# @@ -297,6 +306,14 @@ class Simulator(object): """Hide the GUI.""" self.render(False) + def get_time_step(self): + """Get the time step in the simulator. + + Returns: + float: time step in the simulator + """ + pass + def set_time_step(self, time_step): """Set the time step in the simulator. diff --git a/pyrobolearn/tools/interfaces/controllers/playstation.py b/pyrobolearn/tools/interfaces/controllers/playstation.py index d59ff5a..25cda1c 100755 --- a/pyrobolearn/tools/interfaces/controllers/playstation.py +++ b/pyrobolearn/tools/interfaces/controllers/playstation.py @@ -33,7 +33,7 @@ class PSControllerInterface(GameControllerInterface): [2] Hardware support: https://inputs.readthedocs.io/en/latest/user/hardwaresupport.html """ - def __init__(self, use_thread=False, controller_name='Sony Interactive Entertainment Wireless Controller'): + def __init__(self, use_thread=False, sleep_dt=0, verbose=False, controller_name='Sony'): # Check if some gamepads are connected to the computer gamepads = devices.gamepads if len(gamepads) == 0: @@ -46,6 +46,9 @@ class PSControllerInterface(GameControllerInterface): self.gamepad = gamepad break + if verbose: + print(self.gamepad.name + ' detected.') + if self.gamepad is None: raise ValueError("The specified gamepad/controller was not detected.") @@ -65,7 +68,7 @@ class PSControllerInterface(GameControllerInterface): # last updated button self.last_updated_button = None - super(PSControllerInterface, self).__init__(use_thread) + super(PSControllerInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose) ############## # Properties # @@ -165,7 +168,7 @@ class PSControllerInterface(GameControllerInterface): self.__setitem(event_type, self.map.get(code), state) # display info - if self.verbose: + if self.verbose and self.last_updated_button is not None: print("Pushed button {} - state = {}".format(self.last_updated_button, self.buttons[self.last_updated_button])) @@ -218,8 +221,9 @@ class PSControllerInterface(GameControllerInterface): self.last_updated_button = 'Dpad' elif key == 'LT' or key == 'RT': # max 1023 self.buttons[key] = value / 1023. - self.last_updated_button = key + # self.last_updated_button = key elif event_type == 'Key': + print(event_type, key, value) self.buttons[key] = value self.last_updated_button = key @@ -237,8 +241,8 @@ class PS3ControllerInterface(PSControllerInterface): [2] Hardware support: https://inputs.readthedocs.io/en/latest/user/hardwaresupport.html """ - def __init__(self, use_thread=False): - super(PS3ControllerInterface, self).__init__(use_thread=use_thread, + def __init__(self, use_thread=False, sleep_dt=0, verbose=False,): + super(PS3ControllerInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose, controller_name='Sony PLAYSTATION(R)3 Controller') @@ -255,15 +259,16 @@ class PS4ControllerInterface(PSControllerInterface): [2] Hardware support: https://inputs.readthedocs.io/en/latest/user/hardwaresupport.html """ - def __init__(self, use_thread=False): - super(PS4ControllerInterface, self).__init__(use_thread=use_thread, + def __init__(self, use_thread=False, sleep_dt=0, verbose=False,): + super(PS4ControllerInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose, controller_name='Sony Interactive Entertainment Wireless ' 'Controller') # Tests if __name__ == '__main__': - device = devices.gamepads[0] + device = devices.gamepads[1] + print(device.name) while True: events = device.read() # blocking=False) # get_gamepad() for event in events: diff --git a/pyrobolearn/tools/interfaces/controllers/xbox.py b/pyrobolearn/tools/interfaces/controllers/xbox.py index 54a76c6..0edce40 100644 --- a/pyrobolearn/tools/interfaces/controllers/xbox.py +++ b/pyrobolearn/tools/interfaces/controllers/xbox.py @@ -1,7 +1,21 @@ #!/usr/bin/env python """Provide the Xbox controller interfaces. -This provides the interfaces for the PlayStation controllers (Xbox 360 and Xbox One) using the `inputs` library. +This provides the interfaces for the Xbox controllers (Xbox 360 and Xbox One) using the `inputs` library. + +Troubleshooting: +If the Xbox controller is not detected, please install the necessary driver. On Ubuntu 16.04, you can install the +`xpad` driver by typing the following commands (see [1]): + +```bash +sudo apt-get install git +sudo apt-get install dkms +sudo git clone https://github.com/paroj/xpad.git /usr/src/xpad-0.4 +sudo dkms install -m xpad -v 0.4 +``` + +References: + [1] https://askubuntu.com/questions/783587/how-do-i-get-an-xbox-one-controller-to-work-with-16-04-not-steam """ try: @@ -73,6 +87,9 @@ class XboxControllerInterface(GameControllerInterface): if self.gamepad is None: raise ValueError("The specified gamepad/controller was not detected.") + if verbose: + print(self.gamepad.name + ' detected.') + # translation buttons = ['BTN_SOUTH', 'BTN_EAST', 'BTN_WEST', 'BTN_NORTH', 'BTN_THUMBL', 'BTN_THUMBR', 'BTN_TL', 'BTN_TR', 'BTN_START', 'BTN_SELECT', 'ABS_Z', 'ABS_RZ', 'ABS_HAT0X', 'ABS_HAT0Y', 'ABS_X', 'ABS_Y', 'ABS_RX', @@ -185,7 +202,7 @@ class XboxControllerInterface(GameControllerInterface): self.__setitem(event_type, self.map.get(code), state) # display info - if self.verbose: + if self.verbose and self.last_updated_button is not None: print("Pushed button {} - state = {}".format(self.last_updated_button, self.buttons[self.last_updated_button])) @@ -249,8 +266,9 @@ class Xbox360ControllerInterface(XboxControllerInterface): """ - def __init__(self, use_thread=False): - super(Xbox360ControllerInterface, self).__init__(use_thread=use_thread, controller_name='X-Box 360') + def __init__(self, use_thread=False, sleep_dt=0, verbose=False): + super(Xbox360ControllerInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, + verbose=verbose, controller_name='X-Box 360') class XboxOneControllerInterface(XboxControllerInterface): @@ -258,8 +276,9 @@ class XboxOneControllerInterface(XboxControllerInterface): """ - def __init__(self, use_thread=False): - super(XboxOneControllerInterface, self).__init__(use_thread=use_thread, controller_name='X-Box One') + def __init__(self, use_thread=False, sleep_dt=0, verbose=False): + super(XboxOneControllerInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, + verbose=verbose, controller_name='X-Box One') # Tests diff --git a/pyrobolearn/tools/interfaces/vr/oculus.py b/pyrobolearn/tools/interfaces/vr/oculus.py index 2d89e19..99c60e3 100644 --- a/pyrobolearn/tools/interfaces/vr/oculus.py +++ b/pyrobolearn/tools/interfaces/vr/oculus.py @@ -25,7 +25,7 @@ from pyrobolearn.tools.interfaces.vr import VRInterface __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "GNU GPLv3" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" @@ -153,7 +153,7 @@ class OculusInterface(VRInterface): # self.worldCamera.add_yaw_pitch(yaw, pitch, radian=False) # print(pitch, yaw) pos = self.world_camera.target_position - dist = self.world_camera.dist + dist = self.world_camera.distance elif name == 'BA': # button A: [touch, button] pass elif name == 'BB': # button B: [touch, button] diff --git a/pyrobolearn/worlds/world.py b/pyrobolearn/worlds/world.py index 040166c..da642d4 100644 --- a/pyrobolearn/worlds/world.py +++ b/pyrobolearn/worlds/world.py @@ -19,6 +19,7 @@ from pyrobolearn.simulators import Simulator from pyrobolearn.worlds.world_camera import WorldCamera # from pyrobolearn.utils import has_method, has_variable from pyrobolearn.robots import Body, Robot, robot_names_to_classes +from pyrobolearn.utils.transformation import get_quaternion_from_rpy # TODO: to install the `gdal` library, run the script `pyrobolearn/scripts/install_gdal.sh`, by default do not # import it from pyrobolearn.worlds.utils.heightmaps.diamond_square import diamond_square_heightmap, diamond_square_heightmap_2 @@ -452,7 +453,8 @@ class World(object): 'an instance of Robot'.format(type(robot))) self.bodies[robot.id] = robot - self.ids[robot.id] = [robot] + # self.ids[robot.id] = [robot] + self.ids[robot.id] = [self.__get_method_and_parameters(frame=inspect.currentframe())] return robot def is_body_id(self, body_id): @@ -952,7 +954,9 @@ class World(object): int: unique id of the floor in the world """ # self.floor_id = self.sim.load_urdf('plane100.urdf', use_fixed_base=True, scale=scaling) - self.floor_id = self.sim.load_urdf('plane.urdf', use_fixed_base=True, scale=scaling) + self.floor_id = self.sim.load_urdf('plane.urdf', position=[0., 0., 0.], use_fixed_base=True, scale=scaling) + # distance = self.camera.distance + # self.camera.reset(distance=scaling * distance) return self.floor_id def load_terrain(self, heightmap, position=(0., 0., 0.), orientation=(.707, 0, 0, .707), scaling=1., @@ -1067,7 +1071,7 @@ class World(object): return heightmap # aliases - loadDEM = load_heightmap + load_dem = load_heightmap @staticmethod def generate_heightmap(algo=2, filename=None, width=256, height=256, n=8, min_height=0, max_height=255, noise=0, @@ -1739,8 +1743,9 @@ class World(object): return_body=False): pass - # TODO: add an orientation_range - def distribute(self, body, size=2, position_range=(-1, 1), return_body=False, *args, **kwargs): + # TODO: check collisions when distributing the various bodies (need to know the dimensions) + def distribute(self, body, size=2, position_range=(-1, 1), rpy_range=(0, 0), return_body=False, *args, + **kwargs): r""" Spawn several bodies in the specified range. @@ -1754,6 +1759,9 @@ class World(object): position_range (tuple of float, tuple of np.array): range of the uniform distribution interval for the position of each body. The first element is the lower boundary, and the second one the higher boundary of the interval. + rpy_range (tuple of float, tuple of np.array): range of the uniform distribution interval for the + orientation (expressed as roll-pitch-yaw angles) of each body. The first element is the lower boundary, + and the second one the higher boundary of the interval. return_body (bool): if True, it will return an instance of the `Body`, otherwise, it will return the unique id. *args: list of arguments to be given to :attr:`body` if this last one is callable. @@ -1776,6 +1784,10 @@ class World(object): else: positions = np.random.uniform(low=low, high=high, size=(size, len(high))) + # create orientations (using uniform distribution) + low, high = rpy_range + rpys = np.random.uniform(low=low, high=high, size=(size, 3)) + # check given body argument bodies = [] if self.is_body_id(body): # unique id @@ -1783,9 +1795,10 @@ class World(object): elif isinstance(body, Body): # Body pass elif callable(body) and hasattr(self, body.__name__) and 'position' in inspect.getargspec(body).args: - body = body(position=positions[0], *args, **kwargs) - body = self.wrap(body, wrapper=Body) - positions = positions[1:] + body = body(position=positions[0], orientation=get_quaternion_from_rpy(rpys[0]), *args, **kwargs) + if not isinstance(body, Body): + body = self.wrap(body, wrapper=Body) + positions, rpys = positions[1:], rpys[1:] else: raise TypeError("Expecting the given `body` to be a unique id (int), an instance of Body, or a method of " "`World`, instead got: {} (type={})".format(body, type(body))) @@ -1801,11 +1814,16 @@ class World(object): method = getattr(self, method_name) kwargs = dict(kwargs) + if isinstance(body, Robot): # if robot, get the class + if 'robot' in kwargs: + kwargs['robot'] = kwargs['robot'].__class__ + # distribute the various other bodies # TODO: check for collisions - for position in positions: + for position, rpy in zip(positions, rpys): # update new position and create body kwargs['position'] = position + kwargs['orientation'] = get_quaternion_from_rpy(rpy) body = method(**kwargs) if return_body: @@ -1944,10 +1962,10 @@ class DRCWorld(World): if __name__ == '__main__': import numpy as np from itertools import count - from pyrobolearn.simulators import BulletSim + from pyrobolearn.simulators import Bullet # create simulator - sim = BulletSim() + sim = Bullet() # create world world = BasicWorld(sim) diff --git a/pyrobolearn/worlds/world_camera.py b/pyrobolearn/worlds/world_camera.py index 6b30c51..8c3dd27 100644 --- a/pyrobolearn/worlds/world_camera.py +++ b/pyrobolearn/worlds/world_camera.py @@ -192,8 +192,8 @@ class WorldCamera(object): Set the yaw angle of the camera in radian. The yaw angle is positive when looking on the left and negative when looking on the right. """ - pitch, dist, target_position = self.sim.get_debug_visualizer()[-3:] - self.reset(dist, yaw, pitch, target_position) + pitch, distance, target_position = self.sim.get_debug_visualizer()[-3:] + self.reset(distance, yaw, pitch, target_position) @property def pitch(self): @@ -208,23 +208,23 @@ class WorldCamera(object): Set the pitch angle of the camera in radian. The pitch angle is negative when looking down and positive when looking up. """ - yaw, _, dist, target_position = self.sim.get_debug_visualizer()[-4:] - self.reset(dist, yaw, pitch, target_position) + yaw, _, distance, target_position = self.sim.get_debug_visualizer()[-4:] + self.reset(distance, yaw, pitch, target_position) @property - def dist(self): + def distance(self): """ Return the distance between the camera and the camera target. """ return self.sim.get_debug_visualizer()[10] - @dist.setter - def dist(self, dist): + @distance.setter + def distance(self, distance): """ Set the distance of the camera (in meter) with respect to the target position. """ yaw, pitch, _, target_position = self.sim.get_debug_visualizer()[-4:] - self.reset(dist, yaw, pitch, target_position) + self.reset(distance, yaw, pitch, target_position) @property def target_position(self): @@ -238,8 +238,8 @@ class WorldCamera(object): """ Set the target position of the camera in the Cartesian world space coordinates. """ - yaw, pitch, dist = self.sim.get_debug_visualizer()[-4:-1] - self.reset(dist, yaw, pitch, position) + yaw, pitch, distance = self.sim.get_debug_visualizer()[-4:-1] + self.reset(distance, yaw, pitch, position) @property def position(self): @@ -257,12 +257,12 @@ class WorldCamera(object): """ target = self.target_position vector = (target - position) - dist = np.sqrt(np.sum(vector**2)) - vector = vector / dist + distance = np.sqrt(np.sum(vector**2)) + vector = vector / distance pitch = np.arcsin(vector[2]) # [-pi/2, pi/2] # pitch = np.arctan2(vector[2], vector[1]) yaw = np.arctan2(vector[1], vector[0]) # [-pi, pi] - self.reset(dist, yaw, pitch, target) + self.reset(distance, yaw, pitch, target) @property def orientation(self): @@ -291,9 +291,9 @@ class WorldCamera(object): "angles, instead got: {}".format(orientation)) # reset the camera - dist, target_position = self.sim.get_debug_visualizer()[-2:] + distance, target_position = self.sim.get_debug_visualizer()[-2:] _, pitch, yaw = rpy - self.reset(dist, yaw, pitch, target_position) + self.reset(distance, yaw, pitch, target_position) ########### # Methods # @@ -448,8 +448,8 @@ class WorldCamera(object): """ if radian: yaw, pitch = np.rad2deg(yaw), np.rad2deg(pitch) - dist, target_pos = self.sim.get_debug_visualizer()[-2:] - self.sim.reset_debug_visualizer(dist, yaw, pitch, target_pos) + distance, target_pos = self.sim.get_debug_visualizer()[-2:] + self.sim.reset_debug_visualizer(distance, yaw, pitch, target_pos) def add_yaw_pitch(self, dyaw, dpitch, radian=True): """ @@ -460,12 +460,12 @@ class WorldCamera(object): dpitch (float): small amount to add to the camera's current pitch angle radian (bool): If the given pitch and yaw angles are in radian. """ - yaw, pitch, dist, target_pos = self.sim.get_debug_visualizer()[-4:] + yaw, pitch, distance, target_pos = self.sim.get_debug_visualizer()[-4:] if radian: dyaw, dpitch = np.rad2deg(dyaw), np.rad2deg(dpitch) yaw += dyaw pitch += dpitch - self.sim.reset_debug_visualizer(dist, yaw, pitch, target_pos) + self.sim.reset_debug_visualizer(distance, yaw, pitch, target_pos) def get_rgb_image(self): """ @@ -641,10 +641,10 @@ class WorldCamera(object): if __name__ == '__main__': from itertools import count import time - from pyrobolearn.simulators import BulletSim + from pyrobolearn.simulators import Bullet # create simulator - sim = BulletSim() + sim = Bullet() # load floor floor_id = sim.load_urdf('plane.urdf', use_fixed_base=True) @@ -662,13 +662,13 @@ if __name__ == '__main__': position = camera.position yaw = camera.yaw pitch = camera.pitch - dist = camera.dist + distance = camera.distance target_position = camera.target_position print("Position: {}".format(position)) print("Target position: {}".format(target_position)) print("Yaw: {}".format(np.rad2deg(yaw))) print("Pitch: {}".format(np.rad2deg(pitch))) - print("Distance: {}".format(dist)) + print("Distance: {}".format(distance)) print("##########\n") # move camera