update world (add 'distribute' and 'return_body') + add 2 world examples

examples/worlds/README.md

@@ -0,0 +1,14 @@
+## Worlds
+
+This folder contains examples on how to create a world in the simulator, how to load different shapes in it (such 
+as spheres, cubes, and others) with collisions or not, load various objects that are specified in URDFs/SDFs (such 
+as robots), load a terrain from a heightmap, generate a terrain, and other functionalities.
+
+The world is usually created once the simulator has been selected.
+
+Here are the examples that the user can try:
+1. `load_world.py`: load a basic world (i.e. with a floor and gravity enabled) with different objects (only visual, 
+and with collisions) that are movable, fixed, or are moving.
+2. `load_robot.py`: load a robot in a basic world and distribute randomly few objects on the floor.
+3. `load_heightmap.py`: load a terrain from a heightmap (png) and load a robot on it.
+4. `generate_terrain.py`: generate a terrain and distribute randomly few objects on the terrain.

examples/worlds/load_robot.py

@@ -0,0 +1,54 @@
+#!/usr/bin/env python
+"""load a robot in a basic world and distribute randomly few objects on the floor.
+
+Load a basic world (i.e. with a floor and gravity enabled), and load a robot inside of it, and distributed randomly
+some small cubes in front of it.
+
+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 standard libraries
+from itertools import count
+import argparse
+
+# import pyrobolearn
+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='hyq2max')
+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)
+
+# 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.])
+
+# distribute some boxes in the world
+# There are 2 ways to carry this out.
+# 1. create one instance first (the position is decided by the user), and then distribute
+box_id = world.load_box(position=[2., 0, 0.05], mass=0.1, dimensions=[0.05, 0.05, 0.05], color=(1, 0, 0, 1))
+box_ids1 = world.distribute(box_id, size=10, position_range=([1, -1, 0.05], [3, 1, 0.05]))
+# 2. directly distribute by passing the function as an argument to `world.distribute`
+box_ids2 = world.distribute(world.load_box, size=10, position_range=([-1, -3, 0.05], [1, -1, 0.05]), mass=0.1,
+                            dimensions=[0.05, 0.05, 0.05], color=(0, 1, 0, 1))
+
+# run simulator
+for t in count():
+    # perform one step in the world
+    world.step(sleep_dt=1. / 240)

examples/worlds/load_world.py

@@ -0,0 +1,77 @@
+#!/usr/bin/env python
+"""Load a basic world with different objects in it
+
+Load a basic world (i.e. with a floor and gravity enabled) with different objects (only visual, and with collision
+shapes) that are movable, fixed, or are moving. The objects with a positive mass and a collision shape can be moved
+in the simulator using the mouse.
+
+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 standard libraries
+from itertools import count
+import numpy as np
+
+# import pyrobolearn
+import pyrobolearn as prl
+from pyrobolearn.utils.transformation import get_quaternion
+
+
+# create simulator
+sim = prl.simulators.Bullet()
+
+# create basic world (with a floor and gravity enabled by default)
+world = prl.worlds.BasicWorld(sim)
+
+
+# load basic shapes (only visual and with collisions) #
+
+# load a visual sphere (without collision shape)
+# By setting `return_body` to True, it returns an instance of `Body` which have several methods
+# and attributes, and will allow us to move the robot by setting its position
+sphere = world.load_visual_sphere(position=[1., 0, 1.], radius=0.5, color=(1, 0, 0, 0.5), return_body=True)
+
+# load a movable cylinder (with collision shape)
+cylinder_id = world.load_cylinder(position=[0, -1, 1], color=(1, 0, 0, 1))
+
+# load a movable box (with collision shape)
+box_id = world.load_box(position=[-1, 0, 1], dimensions=[1., 1., 1.], color=(0, 0, 1, 1))
+
+# load an non-movable ellipsoid (with collision shape). To make an object non-movable, set its mass to 0.
+ellipsoid_id = world.load_ellipsoid(position=[0, 0, 2], mass=0, scale=[2., 1., 1.], color=(1, 1, 0, 1))
+
+# load a movable capsule (with collision shape)
+capsule_id = world.load_capsule(position=[0, 1, 1], orientation=get_quaternion([0, np.deg2rad(10), 0]),
+                                color=[0, 1, 1, 1])
+
+# load a visual cone (without collision shape)
+cone_id = world.load_visual_cone(position=[1, 0, 0.5], orientation=(0, 1, 0, 0), scale=(1., 1., 1.),
+                                 color=(1, 1, 1, 0.8))
+
+# load a table via its urdf (the urdf is in the `pybullet_data`)
+table_id = world.load_urdf('table/table.urdf', position=[2, 1.5, 0])
+
+# run simulator
+red = True
+for t in count():
+
+    # move the visual sphere
+    sphere.position = np.array([np.cos(0.006283 * t), np.sin(0.006283 * t), 1.])
+
+    # change the color of the sphere after one complete revolution
+    if t % 1000 == 0:
+        if red:
+            world.change_body_color(sphere.id, (1, 0, 0, 0.5))  # red
+        else:
+            world.change_body_color(sphere.id, (0, 0, 1, 0.5))  # blue
+        red = not red
+
+    # perform one step in the world
+    world.step(sleep_dt=1. / 240)