wassname/pyrobolearn
1
0
Fork 0
mirror of https://github.com/wassname/pyrobolearn.git synced 2026-09-09 11:31:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

clean/update world and world camera

Browse Source
This commit is contained in:
Brian Delhaisse
2019-04-23 01:14:34 +02:00
parent 2088cd5e57
commit 002ed26e31
7 changed files with 811 additions and 491 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pyrobolearn/robots/robot.py
+4 -4
View File
@@ -166,7 +166,7 @@ class Robot(ControllableBody):
Returns:
float[3]: position
np.float[4]: orientation
np.float[4]: orientation (x, y, z, w)
"""
return self.sim.get_base_pose(self.id)
@@ -181,10 +181,10 @@ class Robot(ControllableBody):
def get_base_orientation(self):
"""
Get the base orientation.
Get the base orientation in the form of a quaternion (x, y, z, w).
Returns:
quaternion (np.float[4]): base orientation in the form of a quaternion.
quaternion (np.float[4]): base orientation in the form of a quaternion (x, y, z, w).
"""
return self.sim.get_base_orientation(self.id)
@@ -403,7 +403,7 @@ class Robot(ControllableBody):
[12] str: name of the link (as specified in the URDF/SDF/etc file)
[13] float[3]: joint axis in local frame (ignored for JOINT_FIXED)
[14] float[3]: joint position in parent frame
[15] float[4]: joint orientation in parent frame
[15] float[4]: joint orientation in parent frame (x, y, z, w)
[16] int: parent link index, -1 for base
if multiple joints: list of joint information (i.e. list of above)
pyrobolearn/simulators/bullet.py
+2 -2
View File
@@ -3431,8 +3431,8 @@ class Bullet(Simulator):
"""Get information about the debug visualizer camera.
Returns:
float: width of the visualizer camera
float: height of the visualizer camera
int: width of the visualizer camera
int: height of the visualizer camera
np.float[4,4]: view matrix [4,4]
np.float[4,4]: perspective projection matrix [4,4]
np.float[3]: camera up vector expressed in the Cartesian world space
pyrobolearn/worlds/__init__.py
+4 -1
View File
@@ -1,3 +1,6 @@
# import world camera
from .world_camera import WorldCamera
# import worlds
from .world import World, BasicWorld, WorldCamera
from .world import World, BasicWorld
pyrobolearn/worlds/utils/heightmap_generator.py
+2 -2
View File
@@ -260,7 +260,7 @@ def heightmap_rbf(init_values, x, y, function='multiquadric', lower_bound=0, upp
return heightmap
def heighmap_equation(x, y, z, lower_bound=0, upper_bound=255, dtype=np.int):
def heightmap_equation(x, y, z, lower_bound=0, upper_bound=255, dtype=np.int):
r"""
Generate heightmap from 3D equation :math:`z = f(x,y)`.
@@ -414,7 +414,7 @@ if __name__ == '__main__':
# generate heightmap using RBF interpolations
x = np.array(range(256))
y = np.array(range(256)) # range(128)
y = np.array(range(256)) # range(128)
N_init = 20 # number of bumps
x_init = np.random.randint(low=x.min(), high=x.max(), size=N_init)
y_init = np.random.randint(low=y.min(), high=y.max(), size=N_init)
pyrobolearn/worlds/utils/terrain_generator.py
+62 -57
View File
@@ -9,8 +9,6 @@ References:
[1] https://github.com/deltabrot/random-terrain-generator
"""
import sys
import os
import time
from PIL import Image
import numpy as np
@@ -105,7 +103,6 @@ def diamond_square_heightmap(n, max_height, jitter, jitter_factor):
stride = int((size-1) / 2**(i+1))
radius = int((size-1) / 2**i)
#
for j in range(2**i):
for k in range(2**i):
height = (heightmap[j*radius, k*radius] + heightmap[2*stride + j*radius, k*radius] +
@@ -165,9 +162,9 @@ def diamond_square_heightmap(n, max_height, jitter, jitter_factor):
for i in range(size):
for j in range(size):
if heightmap[i, j] > middle_point:
pixels[i,j] = (0, int(255 * (heightmap[i, j] - middle_point) / (dist/2.) ), 0)
pixels[i, j] = (0, int(255 * (heightmap[i, j] - middle_point) / (dist/2.)), 0)
else:
pixels[i,j] = (10, 10, 200)
pixels[i, j] = (10, 10, 200)
# save image
img.save("map.bmp")
@@ -175,8 +172,27 @@ def diamond_square_heightmap(n, max_height, jitter, jitter_factor):
return heightmap
def create_hexagonal_terrain(segment, scale, tile, max_height, min_height, heightmap, smooth=True, verbose=True,
output_rate=1):
def create_hexagonal_terrain(segment, scale, tile, min_height, heightmap, smooth=True, verbose=True, verbose_rate=1):
"""
Create the terrain with hexagonal tiles.
Args:
segment (int): number of segments; number of square tiles in rows or columns.
scale (float): scaling factor.
tile (bool): if True, it will create a tile texture.
min_height (float): minimum height.
heightmap (np.float[size, size]): 2D square heightmap
smooth (bool): if the normals should be smooth.
verbose (bool): if True, it will output information about the creation of the terrain.
verbose_rate (int): if :attr:`verbose` is True, it will output
Returns:
list: vertices (for OBJ)
list: textures (for OBJ)
list: (smooth) normals (for OBJ)
list: faces (for OBJ)
"""
scale = float(scale)
vertices = []
vertices_obj = []
textures_obj = []
@@ -186,10 +202,7 @@ def create_hexagonal_terrain(segment, scale, tile, max_height, min_height, heigh
prevent_output = False
if tile:
textures_obj.append([0, 0])
textures_obj.append([0, 1])
textures_obj.append([1, 0])
textures_obj.append([1, 1])
textures_obj = [[0, 0], [0, 1], [1, 0], [1, 1]]
for i in range(segment):
if i == segment-1:
@@ -198,12 +211,12 @@ def create_hexagonal_terrain(segment, scale, tile, max_height, min_height, heigh
for j in range(segment):
if not smooth:
tmp = 2 * (i*segment + j)
facesOBJ.append(str(i*(segment+1) + j + 1) + '/' + str(1) + '/' + str(tmp + 1) + ' ' +\
str(i*(segment+1) + j + 2) + '/' + str(2) + '/' + str(tmp + 1) + ' ' +\
str((i+1)*(segment+1) + j + 1) + '/' + str(3) + '/' + str(tmp + 1) )
facesOBJ.append(str((i+1)*(segment+1) + j + 1) + '/' + str(3) + '/' + str(tmp + 2) + ' ' +\
str(i*(segment+1) + j + 2) + '/' + str(2) + '/' + str(tmp + 2) + ' ' +\
str((i+1)*(segment+1) + j + 2) + '/' + str(4) + '/' + str(tmp + 2) )
facesOBJ.append(str(i*(segment+1) + j + 1) + '/' + str(1) + '/' + str(tmp + 1) + ' ' +
str(i*(segment+1) + j + 2) + '/' + str(2) + '/' + str(tmp + 1) + ' ' +
str((i+1)*(segment+1) + j + 1) + '/' + str(3) + '/' + str(tmp + 1))
facesOBJ.append(str((i+1)*(segment+1) + j + 1) + '/' + str(3) + '/' + str(tmp + 2) + ' ' +
str(i*(segment+1) + j + 2) + '/' + str(2) + '/' + str(tmp + 2) + ' ' +
str((i+1)*(segment+1) + j + 2) + '/' + str(4) + '/' + str(tmp + 2))
else:
facesOBJ.append(str(i*(segment+1) + j + 1) + '/' + str(1) + '/' + str(i*(segment+1) + j + 1) + ' ' +
@@ -214,9 +227,9 @@ def create_hexagonal_terrain(segment, scale, tile, max_height, min_height, heigh
' ' + str((i+1)*(segment+1) + j + 2) + '/' + str(4) + '/' +
str((i+1)*(segment+1) + j + 2))
#T1
half_scale = scale/2
scale_seg = scale/segment
# T1
half_scale = scale / 2.
scale_seg = scale / segment
vertices_obj.append([-half_scale + i*scale_seg, heightmap[i, j], -half_scale + j*scale_seg])
if j == segment-1:
vertices_obj.append([-half_scale + i*scale_seg, heightmap[i, j+1], -half_scale + (j+1)*scale_seg])
@@ -259,10 +272,10 @@ def create_hexagonal_terrain(segment, scale, tile, max_height, min_height, heigh
normals_obj.append(normal)
if verbose:
if (time.time() % output_rate) < 0.05 and not prevent_output:
if (time.time() % verbose_rate) < 0.05 and not prevent_output:
display_loading(i*segment + j, segment*segment, "TER | Segm")
prevent_output = True
elif time.time() % output_rate > 0.05:
elif time.time() % verbose_rate > 0.05:
prevent_output = False
# smooth the normals
@@ -324,7 +337,7 @@ def create_hexagonal_terrain(segment, scale, tile, max_height, min_height, heigh
return [vertices_obj, textures_obj, normals_obj, facesOBJ]
def create_obj(vertices, textures, normals, faces):
def create_obj(vertices, textures, normals, faces, filename=None):
"""
Create content of the OBJ file.
@@ -333,10 +346,13 @@ def create_obj(vertices, textures, normals, faces):
textures (list): list of textures
normals (list): list of normals
faces (list): list of faces
filename (None, str): if a string is provided, it will save the OBJ file in the given file path.
Returns:
str: content of the OBJ file.
"""
# create obj list
obj = []
for v in vertices:
obj.append("v " + str(v[0]) + " " + str(v[1]) + " " + str(v[2]))
@@ -346,53 +362,42 @@ def create_obj(vertices, textures, normals, faces):
obj.append("vn " + str(n[0]) + " " + str(n[1]) + " " + str(n[2]))
for f in faces:
obj.append("f " + f)
return '\n'.join(obj)
# create document
obj = '\n'.join(obj)
# create file if specified
if filename is not None:
with open(filename, "w+") as f:
f.write(obj)
return obj
segments = 8
scale = 600
scale = 600.
tile = True
max_height = 75
min_height = 0
max_height = 75.
min_height = 0.
verbose = True
output_rate = 1
jitter = 40
verbose_rate = 1
jitter = 40.
jitter_factor = 1.5
smooth = True
for i in range(len(sys.argv)):
if sys.argv[i] == '-s' or sys.argv[i] == '--segment':
segments = int(sys.argv[i+1])
if sys.argv[i] == '-z' or sys.argv[i] == '--scale':
scale = float(sys.argv[i+1])
if sys.argv[i] == '-m' or sys.argv[i] == '--min':
min_height = float(sys.argv[i+1])
if sys.argv[i] == '-x' or sys.argv[i] == '--max':
max_height = float(sys.argv[i+1])
if sys.argv[i] == '-v' or sys.argv[i] == '--verbose':
verbose = bool(sys.argv[i+1])
if sys.argv[i] == '-r' or sys.argv[i] == '--rate':
output_rate = float(sys.argv[i+1])
if sys.argv[i] == '-j' or sys.argv[i] == '--jitter':
jitter = float(sys.argv[i+1])
if sys.argv[i] == '-f' or sys.argv[i] == '--factor':
jitter_factor = float(sys.argv[i+1])
if sys.argv[i] == '-e' or sys.argv[i] == '--edges':
smooth = bool(sys.argv[i+1])
# create heightmap, generate terrain from it, and obj mesh
print("Generating terrain")
start = time.time()
# create heightmap
heightmap = diamond_square_heightmap(segments, max_height, jitter, jitter_factor)
terrain = create_hexagonal_terrain(2**segments, scale, tile, max_height, min_height, heightmap, smooth, verbose,
output_rate)
OBJ = create_obj(terrain[0], terrain[1], terrain[2], terrain[3])
# create vertices, textures, normals, and faces
terrain = create_hexagonal_terrain(2 ** segments, scale, tile, min_height, heightmap, smooth, verbose,
verbose_rate)
# create obj based on above information
obj = create_obj(vertices=terrain[0], textures=terrain[1], normals=terrain[2], faces=terrain[3],
filename='terrain.obj')
end = time.time()
print("Terrain generated in {:.2f} seconds.".format(end - start))
# save terrain
file = open("terrain.obj", "w+")
file.write(OBJ)
file.close()
pyrobolearn/worlds/world.py
+115 -425
View File
@@ -6,23 +6,22 @@ Dependencies:
- `pyrobolearn.simulators`
"""
import numpy as np
import collections
import inspect
import multiprocessing
import os
import numpy as np
import cv2
import time
from pyrobolearn.simulators import Simulator
from pyrobolearn.worlds.world_camera import WorldCamera
from pyrobolearn.utils.converter import QuaternionListConverter
# from pyrobolearn.utils.heightmap_generator import * # TODO: problem with gdal installation
from pyrobolearn.utils import has_method, has_variable
from pyrobolearn.robots import Robot, robot_names_to_classes
# from pyrobolearn.tools.bridges.bridge import Bridge
from pyrobolearn.robots import Body, Robot, robot_names_to_classes
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
@@ -34,341 +33,6 @@ __email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class WorldCamera(object):
r"""World camera.
Camera that looks at the world (only available in the simulator).
The following operations carried out (in the given order) by OpenGL in order to display images seen by the
camera are:
* M: Model space --> World space. This transforms the coordinates of each model described in their own local
frame :math:`[x_{l}, y_{l}, z_{l}, 1]` to world coordinates :math:`[x_{w}, y_{w}, z_{w}, 1]`.
* V: World space --> View space. This transforms world coordinates :math:`[x_{w}, y_{w}, z_{w}, 1]` into eye
coordinates :math:`[x_{e}, y_{e}, z_{e}, 1]`. That is, it rotates and translates the world such that it is in
front of the camera.
* P: View space --> Projection space. Transforms the eye coordinates into clip coordinates using an orthographic
or perspective projection. The new coordinates are given by :math:`[x_{c}, y_{c}, z_{c}, w_{c}]`. This is not
normalized, i.e. w_{c} is not equal to 1. See next operation.
* norm: Screen space --> NDC space. This normalizes the previous clipped coordinates into
Normalized Device Coordinates (NDC) where each coordinate is normalized and is between -1 and 1. That is,
we now have :math:`[x_{n}, y_{n}, z_{n}, 1] = [x_c/w_c, y_c/w_c, z_c, w_c/w_c]`
* Vp: NDC space --> Screen space. Finally, this maps the previous normalized clip coordinates to pixel
coordinates :math:`[x_{s}, y_{s}, z_{s}, 1]` where :math:`x_s` (:math:`y_s`) is between 0 and the width
(height) of the screen respectively, and :math:`z_s` represents the depth which is between 0 and 1.
References:
[1] http://www.codinglabs.net/article_world_view_projection_matrix.aspx
[2] https://learnopengl.com/Getting-started/Coordinate-Systems
[3] http://www.thecodecrate.com/opengl-es/opengl-transformation-matrices/
[4] http://learnwebgl.brown37.net/08_projections/projections_perspective.html
"""
def __init__(self, simulator):
self.sim = simulator
def __repr__(self):
return self.__class__.__name__
@property
def info(self):
"""
Return all the information about the camera.
"""
return self.get_debug_visualizer_camera(convert=False)
# alias
def get_debug_visualizer_camera(self, convert=True):
"""
Return all the information provided by the camera.
Args:
convert (bool): if True, it will convert the lists into numpy vectors and matrices
Returns:
width (int): width of the camera image in pixels
height (int): height of the camera image in pixels
view_matrix (float[16], float[4x4]): view matrix of the camera
projection_matrix (float[16], float[4x4]): projection matrix of the camera
camera_up (float[3]): up axis of the camera, in Cartesian world space coordinates
cameraForward (float[3]): forward axis of the camera, in Cartesian world space coordinates
horizontal (float[3]): TBD. This is a horizontal vector that can be used to generate rays (for mouse
picking or creating a simple ray tracer for example)
vertical (float[3]): TBD.This is a vertical vector that can be used to generate rays(for mouse picking
or creating a simple ray tracer for example).
yaw (float): yaw angle of the camera, in Cartesian local space coordinates
pitch (float): pitch angle of the camera, in Cartesian local space coordinates
dist (float): distance between the camera and the camera target
target (float[3]): target of the camera, in Cartesian world space coordinates
"""
return self.sim.get_debug_visualizer()
@property
def width(self):
"""
Return the width of the pictures (in pixel)
"""
return self.sim.get_debug_visualizer()[0]
@property
def height(self):
"""
Return the height of the pictures (in pixel)
"""
return self.sim.get_debug_visualizer()[1]
@property
def V(self):
"""
Return the view matrix, which maps from the world to the view space.
"""
return self.sim.get_debug_visualizer()[2]
# alias
view_matrix = V
@property
def Vinv(self):
"""
Return the inverse of the view matrix
"""
return np.linalg.inv(self.V)
@property
def P(self):
"""
Return the projection matrix, which maps from the view to the projected/clipped space.
"""
return self.sim.get_debug_visualizer()[3]
# alias
projection_matrix = P
@property
def Pinv(self):
"""
Return the inverse of the projection matrix
"""
return np.linalg.inv(self.P)
@property
def Vp(self):
"""
Return the viewport matrix, which maps from the normalized clip coordinates to pixel coordinates.
"""
width, height = self.sim.get_debug_visualizer()[:2]
return np.array([[width / 2, 0, 0, width / 2],
[0, height / 2, 0, height / 2],
[0, 0, 0.5, 0.5],
[0, 0, 0, 1]])
viewport_matrix = Vp
@property
def Vp_inv(self):
"""
Return the inverse of the viewport matrix.
"""
return np.linalg.inv(self.Vp)
def get_matrices(self, inverse=False):
"""
Return the view, projection, and viewport matrices.
"""
width, height, V, P = self.sim.get_debug_visualizer()[:4]
Vp = np.array([[width / 2, 0, 0, width / 2],
[0, height / 2, 0, height / 2],
[0, 0, 0.5, 0.5],
[0, 0, 0, 1]])
if inverse:
Vinv = np.linalg.inv(V)
Pinv = np.linalg.inv(P)
Vpinv = np.linalg.inv(Vp)
return V, P, Vp, Vinv, Pinv, Vpinv
return V, P, Vp
@property
def up_vector(self):
"""
Return the up axis of the camera in the Cartesian world space coordinates
"""
return self.sim.get_debug_visualizer()[4]
@property
def forward_vector(self):
"""
Return the forward axis of the camera in the Cartesian world space coordinates.
"""
return self.sim.get_debug_visualizer()[5]
def get_vectors(self):
"""
Return the forward, up, and lateral vectors of the camera.
"""
up_vector, forward_vector = self.sim.get_debug_visualizer()[4:6]
lateral_vector = np.cross(forward_vector, up_vector)
return forward_vector, up_vector, lateral_vector
@property
def yaw(self):
"""
Return the yaw angle of the camera in radian
"""
return self.sim.get_debug_visualizer()[8]
@property
def pitch(self):
"""
Return the pitch angle of the camera.
"""
return self.sim.get_debug_visualizer()[9]
@property
def dist(self):
"""
Return the distance between the camera and the camera target.
"""
return self.sim.get_debug_visualizer()[10]
@property
def target_position(self):
"""
Return the target of the camera in the Cartesian world space coordinates.
"""
return self.sim.get_debug_visualizer()[11]
@target_position.setter
def target_position(self, pos):
yaw, pitch, dist = self.sim.get_debug_visualizer()[-4:-1]
self.sim.reset_debug_visualizer(dist, yaw, pitch, pos)
@property
def position(self):
"""
Return the current position of the camera in the Cartesian world space coordinates.
"""
Vinv = np.linalg.inv(self.V) # compute inverse of the view matrix
position = Vinv[:3, 3] # the last column is the current position of the camera
return position
@position.setter
def position(self, pos):
self.sim.reset_debug_visualizer(dist, yaw, pitch, targetPos)
@property
def orientation(self):
# based on forward_vector and up_vector
pass
@orientation.setter
def orientation(self, orientation):
pass
def set_yaw_pitch(self, yaw, pitch, radian=True):
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)
def add_yaw_pitch(self, dyaw, dpitch, radian=True):
yaw, pitch, dist, 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)
def get_rgb_image(self):
"""
Return the captured RGB image.
"""
return self.get_rgba_image()[:, :, :3]
def get_rgba_image(self):
"""
Return the captured RGBA image. 'A' stands for alpha channel (for opacity/transparency)
"""
width, height, view_matrix, projection_matrix = self.sim.get_debug_visualizer()[:4]
img = np.array(self.sim.get_camera_image(width, height, view_matrix, projection_matrix)[2])
img = img.reshape(width, height, 4) # RGBA
return img
def get_depth_image(self):
"""
Return the depth image.
"""
width, height, viewMatrix, projectionMatrix = self.sim.get_debug_visualizer()[:4]
img = np.array(self.sim.get_camera_image(width, height, viewMatrix, projectionMatrix)[3])
img = img.reshape(width, height)
return img
def get_rgbad_image(self, concatenate=True):
"""
Return the RGBA and depth images.
"""
width, height, view_matrix, projection_matrix = self.sim.get_debug_visualizer()[:4]
rgba, depth = self.sim.get_camera_image(width, height, view_matrix, projection_matrix)[2:4]
rgba = np.array(rgba).reshape(width, height, 4)
depth = np.array(depth).reshape(width, height)
if concatenate:
return np.dstack((rgba, depth))
return (rgba, depth)
def screen_to_world(self, x_screen, Vp_inv=None, P_inv=None, V_inv=None):
"""
Return the corresponding coordinates in the Cartesian world space from the coordinates of a point
on the screen.
Args:
x_screen (float[4]): augmented vector coordinates of a point on the screen
Vp_inv (float[4,4])): inverse of viewport matrix
P_inv (float[4,4]): inverse of projection matrix
V_inv (float[4,4]): inverse of view matrix
Returns:
float[4]: augmented vector coordinates of the corresponding point in the world
"""
if Vp_inv is None:
Vp_inv = self.Vp_inv
if P_inv is None:
P_inv = self.Pinv
if V_inv is None:
V_inv = self.Vinv
x_ndc = Vp_inv.dot(x_screen)
x_ndc[1] = -x_ndc[1] # invert y-axis
x_ndc[2] = -x_ndc[2] # invert z-axis
x_eye = P_inv.dot(x_ndc)
x_eye = x_eye / x_eye[3] # normalize
x_world = V_inv.dot(x_eye)
return x_world
def world_to_screen(self, x_world, V=None, P=None, Vp=None):
"""
Return the corresponding screen coordinates from a 3D point in the world.
Args:
x_world (float[4]): augmented vector coordinates of a point in the Cartesian world space
V (float[4,4], None): view matrix
P (float[4,4], None): projection matrix
Vp (float[4,4], None): viewport matrix
Returns:
float[4]: augmented vector coordinates of the corresponding point on the screen
"""
if V is None: V = self.V
if P is None: P = self.P
if Vp is None: Vp = self.Vp
x_eye = V.dot(x_world)
x_clip = P.dot(x_eye)
x_ndc = x_clip / x_clip[3] # normalize
x_ndc[1] = -x_ndc[1] # invert y-axis (as y pointing upward in projection but should point downward in screen)
x_ndc[2] = -x_ndc[2] # invert z-axis (to get right-handed coord. system, -1=close and 1=far)
x_screen = Vp.dot(x_ndc) # for depth between 0(=close) and 1(=far)
return x_screen
class World(object):
r"""World class.
@@ -400,6 +64,10 @@ class World(object):
# By default, set the gravity
self.gravity = gravity
# set world camera
self.camera = WorldCamera(self.simulator)
# keep track of the objects present in the world
self.robots = {}
self.movable_bodies = {} # set()
self.immovable_bodies = {} # set()
@@ -409,8 +77,7 @@ class World(object):
self.map = None
self.floor_id = -1
self.quaternion_converter = QuaternionListConverter(convention=1)
# create world state
self.world_state = None
# configure debug visualizer
@@ -437,11 +104,6 @@ class World(object):
"{}".format(type(simulator)))
self.sim = simulator
@property
def main_camera(self):
"""Return the main camera of the simulator."""
return WorldCamera(self.sim)
@property
def gravity(self):
"""Return the gravity vector."""
@@ -557,11 +219,12 @@ class World(object):
if self.floor_id > 0:
self.sim.change_dynamics(body_id=self.floor_id, link_id=-1, **dynamics)
########################
# Operator Overloading #
########################
#############
# Operators #
#############
def __repr__(self):
"""Return a representation string of the object."""
return self.__class__.__name__
def __contains__(self, item):
@@ -577,14 +240,14 @@ class World(object):
if not isinstance(item, int):
item = item.id
return (item in self.robots) or (item in self.movable_bodies) or (item in self.immovable_bodies) \
or (item in self.visual_objects)
return (item in self.robots) or (item in self.movable_bodies) or (item in self.immovable_bodies) or \
(item in self.visual_objects)
###########
# Methods #
###########
def set_bridges(self, bridges):
def set_bridges(self, bridges): # TODO: remove this
"""
This append the given bridges to various interfaces to the list of bridges.
@@ -674,6 +337,21 @@ class World(object):
if sleep_dt is not None:
time.sleep(sleep_dt)
def follow(self, body, distance=None, yaw=None, pitch=None):
"""
Follow the given body with the world camera at the specified distance, yaw and pitch angles.
Args:
body (Body): body to follow with the world camera.
distance (float, None): distance (in meter) from the camera and the body position. If None, it will take
the current distance.
yaw (float, None): camera yaw angle (in radians) left/right. If None, it will take the current yaw angle.
pitch (float, None): camera pitch angle (in radians) up/down. If None, it will take the current pitch angle.
"""
if not isinstance(body, Body):
raise TypeError("Expecting the given body to be an instance of `Body`, instead got: {}".format(type(body)))
self.camera.reset(distance=distance, yaw=yaw, pitch=pitch, target_position=body.position)
def load_robot(self, robot, position=None, orientation=None, fixed_base=None, *args, **kwargs):
"""
Load the robot into the world. If the robot parameter is a known robot name or the path to the urdf file,
@@ -690,6 +368,8 @@ class World(object):
Return:
Robot: instance of the Robot class
"""
# TODO check the height of the terrain where we wish to load the robot
if isinstance(robot, Robot): # the robot is already loaded, then add it to the list
pass
@@ -805,17 +485,17 @@ class World(object):
"""
bodies = self.sim.load_mjcf(filename, scaling=scaling)
for body in bodies:
self.movable_bodies[body] = self.sim.getBodyInfo(body)
self.movable_bodies[body] = self.sim.get_body_info(body)
return bodies
def _loadSDForURDF(self, path, position, orientation, scaling, objectType=None):
def _load_sdf_or_urdf(self, path, position, orientation, scaling, objectType=None):
extension_name = path.split('.')[-1]
if extension_name == 'urdf':
object_id = self.sim.load_urdf(path, position, orientation, scale=scaling)
self.movable_bodies[object_id] = 'urdf' if objectType is None else objectType
elif extension_name == 'sdf':
object_id = self.sim.loadSDF(path, scale=scaling) # list of ids
for i in object_id: # assume for now that the objects are movable...
object_id = self.sim.load_sdf(path, scale=scaling) # list of ids
for i in object_id: # assume for now that the objects are movable...
self.movable_bodies[i] = 'sdf' if objectType is None else objectType
else:
raise ValueError('Extension name of the file is not known; this method only accepts URDF/SDF files.')
@@ -828,10 +508,10 @@ class World(object):
Args:
object_type (str): type of the object (name, 'sphere',
path:
position:
orientation:
scaling:
path (str): path to the object
position (np.float[3]): position of the object in the world frame.
orientation (np.float[4]): orientation of the object in the world frame.
scaling (float): scaling factor
Returns:
int or int[]: object ids
@@ -839,7 +519,7 @@ class World(object):
# check if an object has already been loaded at that place.
if path is not None:
object_id = self._loadSDForURDF(path, position, orientation, scaling=1., objectType=object_type)
object_id = self._load_sdf_or_urdf(path, position, orientation, scaling=1., objectType=object_type)
else:
if object_type == 'sphere':
object_id = self.load_sphere(position)
@@ -1064,7 +744,7 @@ class World(object):
None
"""
# TODO: currently not possible in PyBullet
raise NotImplementedError
pass
def get_object_aabb(self, object_id, link_id=-1):
"""
@@ -1119,11 +799,38 @@ class World(object):
return False
return True
def get_closest_object(self): # Not possible for now
raise NotImplementedError
def get_closest_objects(self, body, radius=1, link_id=-1, body2=None, link2_id=-1): # Not possible for now
"""
Get the closest objects from the specified body (or link) within the specified radius.
def get_closest_objects(self, radius): # Not possible for now
raise NotImplementedError
Args:
body (Body): body.
radius (float): radius around the body in which we check the closest objects.
link_id (int): link id. Only report contact points that involve link index of body A.
body2 (int): only report contact points that involve body B. Important: you need to have a valid body A
if you provide body B
link2_id (int): only report contact points that involve link index of body B
Returns:
list:
int: contact flag (reserved)
int: body unique id of body A
int: body unique id of body B
int: link index of body A, -1 for base
int: link index of body B, -1 for base
np.float[3]: contact position on A, in Cartesian world coordinates
np.float[3]: contact position on B, in Cartesian world coordinates
np.float[3]: contact normal on B, pointing towards A
float: contact distance, positive for separation, negative for penetration
float: normal force applied during the last `step`. Always equal to 0.
float: lateral friction force in the first lateral friction direction (see next returned value)
np.float[3]: first lateral friction direction
float: lateral friction force in the second lateral friction direction (see next returned value)
np.float[3]: second lateral friction direction
"""
if body2 is not None:
return self.sim.get_closest_points(body1=body.id, body2=body2, distance=radius,
link1_id=link_id, link2_id=link_id)
def load_floor(self, scaling=1.):
"""
@@ -1251,7 +958,7 @@ class World(object):
self.sim.change_visual_shape(heightmap, -1, texture_id=texture)
# remove mesh from memory
os.remove(filename + '.obj') # remove mesh from memory
os.remove(filename + '.obj') # remove mesh from memory
os.remove(filename + '.mtl')
# replace the floor if there is already one present
@@ -1264,26 +971,38 @@ class World(object):
# aliases
loadDEM = load_heightmap
def generateHeightmap(self, filename=None, algo=None):
def generate_heightmap(self, filename=None, algo=None):
"""
Generate a heightmap (png) using the specified algorithm. We provide 4 algorithms to generate this last one:
1. Random
2. Diamond algorithm
3.
4.
1. by generating it randomly (not advised)
2. by using the diamond-square algorithm
3. by using gaussian process regression
4. by interpolating the given initial points using RBF functions.
5. from a given 3D equation
6. by using the Geospatial Data Abstraction Library (GDAL), which allows to open Digital Elevation Models
(DEM) or Geographic Information System (GIS). It can open a .tiff, .geotiff, ascii grid, or image
(jpg, png,...) file. Warning: the GDAL option requires the gdal library to be installed.
By default, the diamond algorithm is used.
By default, the diamond-square algorithm is used. Two good other algorithms are the GDAL and the RBF
approaches.
Args:
filename (None, str): if not None, it will save the heightmap in the format specified by the filename.
The format is inferred from the filename. Supported ones include '.png', '.jpg', and '.obj'.
Returns:
np.ndarray: heightmap
np.array[W,H]: heightmap (with, height)
"""
pass
def generate_terrain(self, filename=None):
def generate_terrain(self, heightmap, filename):
"""
Generate the terrain (obj) file and load it in the world.
Args:
heightmap (np.array[W,H]): 2D heightmap.
filename (str): filename.
"""
pass
def load_stadium(self, scaling=1.):
@@ -1323,16 +1042,10 @@ class World(object):
def load_stairs(self):
pass
def createCity(self):
def create_city(self):
pass
def createParkour(self):
pass
def createMountainWithPath(self):
pass
def loadTable(self, position, scaling=1.):
def load_table(self, position, scaling=1.):
"""
Load a table in the world.
@@ -1357,11 +1070,11 @@ class World(object):
Returns:
int: unique id of the shelf
"""
shelf = self.sim.loadSDF('kiva_shelf/model.sdf', scale=scaling)[0]
shelf = self.sim.load_sdf('kiva_shelf/model.sdf', scale=scaling)[0]
self.movable_bodies[shelf] = 'shelf'
return shelf
def load_visual_Sphere(self, position, radius=0.5, color=None):
def load_visual_sphere(self, position, radius=0.5, color=None):
"""
Load a visual sphere in the world (only available in the simulator).
@@ -1900,56 +1613,36 @@ if __name__ == '__main__':
# world = World(sim)
# world.load_bot_lab()
# world.load_sdf('/home/brian/Downloads/cobblestones_origin/model.sdf', scaling=1)
# world.load_mesh('/home/brian/Downloads/cobblestones_origin/mesh/cobblestones.obj',
# position=[0, 0, 0],
# orientation=[.707, 0, 0, .707],
# mass=0.,
# scale=(1., 1., 1.),
# # color=[1, 0, 0, 1],
# flags=1)
# world.load_mesh('/home/brian/save/code/random-terrain-generator-master/terrain.obj',
# # load meshes
# world.load_mesh('meshes/terrain.obj',
# position=[0, 0, -2],
# orientation=[.707, 0, 0, .707],
# mass=0.,
# scale=(.1, .1, .1),
# # color=[1, 0, 0, 1],
# flags=1)
# world.load_mesh('bedroom.obj', [0, 0, 0], mass=0., color=[0.4, 0.4, 0.4, 1], flags=1) #, scale=(0.01, 0.01, 0.01))
# world.load_mesh('mtsthelens.obj', [0, 0, -8], mass=0., color=[0.2, 0.5, 0.2, 1], flags=1, scale=(0.01,0.01,0.01))
# world.load_mesh('meshes/terrain.obj', [0,0,0], mass=0., color=[1,1,1,1], flags=1)
# world.load_mesh('/home/brian/heightmap_old.obj', [0,0,0], mass=0., scale=(0.1,0.1,0.01), color=[1,1,1,1], flags=1)
# world.load_mesh('/home/brian/Downloads/arab_desert/desert.obj',
# position=[0, 0, -10.8], orientation=(0.707,0,0,0.707), mass=0., scale=(1, 1, 1),
# color=[1, 1, 1, 1], flags=1)
# world.load_mesh('/home/brian/PhD-repos/pyrobolearn/tests/heightmap_test_exp.obj', [0, 0, 0], mass=0.,
# scale=(0.1, 0.1, 0.015),
# color=[1, 0, 0, 1],
# flags=1)
# # load robots
# world.load_robot('Cogimon', position=[0,0,1.])
# world.load_robot('coman', use_fixed_base=False)
# sphere = world.load_visual_Sphere([1.,0,1.], color=(1,0,0,0.5))
# load basic shapes
sphere = world.load_visual_sphere([1., 0, 1.], color=(1, 0, 0, 0.5))
# world.load_visual_box([-1,0,1], dimensions=[1.,1.,1.], color=[0,0,1,0.5])
# world.load_cylinder([0, -1, 1], color=[1, 0, 0, 1])
# world.load_capsule([0, 1, 1], color=[1, 0, 0, 1])
# world.load_mesh(filename='duck.obj', [1, 0, 2], [0.707, 0, 0, 0.707], mass=0.1, scale=[0.1,0.1,0.1],
# color=[1, 0, 0, 1])
# from utils.orientation import RotX, RotY, RotZ, getQuaternionFromMatrix
# R = RotZ(np.deg2rad(90.))
# q = tuple(getQuaternionFromMatrix(R))
# world.load_ellipsoid([0,0,2], orientation=q, mass=0, scale=[2.,1.,1.], color=(0,0,1,1))
# world.load_ellipsoid([0,0,2], mass=0, scale=[2.,1.,1.], color=(0,0,1,1))
# world.load_cone([1,1,2])
world.load_right_triangular_prism([-1, -1, 2])
# floor = world.load_mesh(filename='box', [1, 0, 2], mass=0, color=None)
# floor = world.load_floor()
# print(p.get_dynamics_info(floor, -1))
# floor = world.load_mesh([1,0,0], [0,0,0,1], filename='grass.obj', mass=0, color=(1,1,1,1))
# texture = sim.load_texture('grass.png')
# sim.change_visual_shape(floor, -1, texture_id=texture)
@@ -1958,10 +1651,7 @@ if __name__ == '__main__':
# texture = sim.load_texture('grass.png')
# sim.change_visual_shape(grass, -1, texture_id=texture)
# vs = world.load_visual_Sphere([0,0,2], radius=0.1, color=(0,0,1,1))
# path = '/home/brian/bullet3/data/'
# objects = p.load_mjcf(path+"MPL/mpl2.xml")
# vs = world.load_visual_sphere([0,0,2], radius=0.1, color=(0,0,1,1))
# world.load_plane([1.,0.,1.], [0,0,0,1], color=(1,0,0,1))
@@ -1973,12 +1663,12 @@ if __name__ == '__main__':
# p = world.get_object_position(sphere)
# p -= 0.001 * np.array([1.,0,0])
p = np.array([np.cos(w*t), np.sin(w*t), 1.])
# world.move_object(sphere, p)
# if t % T == 0:
# if red:
# world.change_object_color(sphere, (1,0,0,0.5))
# else:
# world.change_object_color(sphere, (0,0,1,0.5))
# red = not red
# world.move_object(sphere)
if t % T == 0:
if red:
world.change_object_color(sphere, (1,0,0,0.5))
else:
world.change_object_color(sphere, (0,0,1,0.5))
red = not red
world.move_object(sphere, p)
world.step(sleep_dt=1./240)
pyrobolearn/worlds/world_camera.py
+622
View File
@@ -0,0 +1,622 @@
#!/usr/bin/env python
"""Define the the `WorldCamera` class.
Get the camera that looks at the world (only available in the simulator).
Dependencies:
- `pyrobolearn.simulators`
"""
import numpy as np
from pyrobolearn.utils.orientation import get_quaternion_from_matrix, get_rpy_from_matrix, get_rpy_from_quaternion
from pyrobolearn.simulators import Simulator
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["Brian Delhaisse"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class WorldCamera(object):
r"""World camera.
Camera that looks at the world (only available in the simulator).
The following operations carried out (in the given order) by OpenGL in order to display images seen by the
camera are:
* M: Model space --> World space. This transforms the coordinates of each model described in their own local
frame :math:`[x_{l}, y_{l}, z_{l}, 1]` to world coordinates :math:`[x_{w}, y_{w}, z_{w}, 1]`.
* V: World space --> View space. This transforms world coordinates :math:`[x_{w}, y_{w}, z_{w}, 1]` into eye
coordinates :math:`[x_{e}, y_{e}, z_{e}, 1]`. That is, it rotates and translates the world such that it is in
front of the camera.
* P: View space --> Projection space. Transforms the eye coordinates into clip coordinates using an orthographic
or perspective projection. The new coordinates are given by :math:`[x_{c}, y_{c}, z_{c}, w_{c}]`. This is not
normalized, i.e. w_{c} is not equal to 1. See next operation.
* norm: Screen space --> NDC space. This normalizes the previous clipped coordinates into
Normalized Device Coordinates (NDC) where each coordinate is normalized and is between -1 and 1. That is,
we now have :math:`[x_{n}, y_{n}, z_{n}, 1] = [x_c/w_c, y_c/w_c, z_c, w_c/w_c]`
* Vp: NDC space --> Screen space. Finally, this maps the previous normalized clip coordinates to pixel
coordinates :math:`[x_{s}, y_{s}, z_{s}, 1]` where :math:`x_s` (:math:`y_s`) is between 0 and the width
(height) of the screen respectively, and :math:`z_s` represents the depth which is between 0 and 1.
References:
[1] http://www.codinglabs.net/article_world_view_projection_matrix.aspx
[2] https://learnopengl.com/Getting-started/Coordinate-Systems
[3] http://www.thecodecrate.com/opengl-es/opengl-transformation-matrices/
[4] http://learnwebgl.brown37.net/08_projections/projections_perspective.html
"""
def __init__(self, simulator):
"""
Initialize the world camera.
Args:
simulator:
"""
self.simulator = simulator
##############
# Properties #
##############
@property
def simulator(self):
"""Return the simulator instance."""
return self.sim
@simulator.setter
def simulator(self, simulator):
"""Set the simulator instance."""
if not isinstance(simulator, Simulator):
raise TypeError("Expecting the given simulator to be an instance of `Simulator`, instead got: "
"{}".format(type(simulator)))
self.sim = simulator
@property
def info(self):
"""
Return all the information about the camera.
"""
return self.get_debug_visualizer_camera()
@property
def width(self):
"""
Return the width of the pictures (in pixel)
"""
return self.sim.get_debug_visualizer()[0]
@property
def height(self):
"""
Return the height of the pictures (in pixel)
"""
return self.sim.get_debug_visualizer()[1]
@property
def V(self):
"""
Return the view matrix, which maps from the world to the view space.
"""
return self.sim.get_debug_visualizer()[2]
# alias
view_matrix = V
@property
def Vinv(self):
"""
Return the inverse of the view matrix
"""
return np.linalg.inv(self.V)
@property
def P(self):
"""
Return the projection matrix, which maps from the view to the projected/clipped space.
"""
return self.sim.get_debug_visualizer()[3]
# alias
projection_matrix = P
@property
def Pinv(self):
"""
Return the inverse of the projection matrix
"""
return np.linalg.inv(self.P)
@property
def Vp(self):
"""
Return the viewport matrix, which maps from the normalized clip coordinates to pixel coordinates.
"""
width, height = self.sim.get_debug_visualizer()[:2]
return np.array([[width / 2., 0, 0, width / 2.],
[0, height / 2., 0, height / 2.],
[0, 0, 0.5, 0.5],
[0, 0, 0, 1]])
viewport_matrix = Vp
@property
def Vp_inv(self):
"""
Return the inverse of the viewport matrix.
"""
return np.linalg.inv(self.Vp)
@property
def up_vector(self):
"""
Return the up axis of the camera in the Cartesian world space coordinates
"""
return self.sim.get_debug_visualizer()[4]
@property
def forward_vector(self):
"""
Return the forward axis of the camera in the Cartesian world space coordinates.
"""
return self.sim.get_debug_visualizer()[5]
@property
def yaw(self):
"""
Return the yaw angle of the camera in radian
"""
return self.sim.get_debug_visualizer()[8]
@yaw.setter
def yaw(self, yaw):
"""
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)
@property
def pitch(self):
"""
Return the pitch angle of the camera in radian
"""
return self.sim.get_debug_visualizer()[9]
@pitch.setter
def pitch(self, pitch):
"""
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)
@property
def dist(self):
"""
Return the distance between the camera and the camera target.
"""
return self.sim.get_debug_visualizer()[10]
@dist.setter
def dist(self, dist):
"""
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)
@property
def target_position(self):
"""
Return the target position of the camera in the Cartesian world space coordinates.
"""
return self.sim.get_debug_visualizer()[11]
@target_position.setter
def target_position(self, position):
"""
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)
@property
def position(self):
"""
Return the current position of the camera in the Cartesian world space coordinates.
"""
Vinv = np.linalg.inv(self.V) # compute inverse of the view matrix
position = Vinv[:3, 3] # the last column is the current position of the camera
return position
@position.setter
def position(self, position):
"""
Set the position of the camera in the world.
"""
target = self.target_position
vector = (target - position)
dist = np.sqrt(np.sum(vector**2))
vector = vector / dist
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)
@property
def orientation(self):
"""
Return the orientation (as a quaternion).
"""
# based on forward_vector and up_vector
Vinv = np.linalg.inv(self.V) # compute inverse of the view matrix
orientation = get_quaternion_from_matrix(Vinv[:3, :3])
return orientation
@orientation.setter
def orientation(self, orientation):
"""
Set the orientation (expressed as a quaternion, rotation matrix, or roll-pitch-yaw angles) of the camera.
"""
# convert the orientation to roll-pitch-yaw angle
if orientation.shape == (4,): # quaternion (x,y,z,w)
rpy = get_rpy_from_quaternion(orientation)
elif orientation.shape == (3, 3): # rotation matrix
rpy = get_rpy_from_matrix(orientation)
elif orientation.shape == (3,): # roll-pitch-yaw angle
rpy = orientation
else:
raise ValueError("Expecting the given orientation to be a quaternion, rotation matrix, or Roll-Pitch-Yaw "
"angles, instead got: {}".format(orientation))
# reset the camera
dist, target_position = self.sim.get_debug_visualizer()[-2:]
_, pitch, yaw = rpy
self.reset(dist, yaw, pitch, target_position)
###########
# Methods #
###########
# alias
def get_debug_visualizer_camera(self):
"""
Return all the information provided by the camera.
Returns:
int: width of the visualizer camera (in pixel)
int: height of the visualizer camera (in pixel)
np.float[4,4]: view matrix [4,4]
np.float[4,4]: perspective projection matrix [4,4]
np.float[3]: camera up vector expressed in the Cartesian world space
np.float[3]: forward axis of the camera expressed in the Cartesian world space
np.float[3]: This is a horizontal vector that can be used to generate rays (for mouse picking or creating
a simple ray tracer for example)
np.float[3]: This is a vertical vector that can be used to generate rays (for mouse picking or creating a
simple ray tracer for example)
float: yaw angle (in radians) of the camera, in Cartesian local space coordinates
float: pitch angle (in radians) of the camera, in Cartesian local space coordinates
float: distance between the camera and the camera target
np.float[3]: target of the camera, in Cartesian world space coordinates
"""
return self.sim.get_debug_visualizer()
def reset(self, distance=None, yaw=None, pitch=None, target_position=None):
"""Reset the debug visualizer camera.
Reset the 3D OpenGL debug visualizer camera distance (between eye and camera target position), camera yaw and
pitch and camera target position
Args:
distance (float, None): distance from eye to camera target position. If None, it will take the current
distance.
yaw (float, None): camera yaw angle (in radians) left/right. If None, it will take the current yaw angle.
pitch (float, None): camera pitch angle (in radians) up/down. If None, it will take the current pitch angle.
target_position (np.float[3], None): target focus point of the camera. If None, it will take the current
target position.
"""
y, p, d, t = self.sim.get_debug_visualizer()[-4:]
if distance is None:
distance = d
if yaw is None:
yaw = y
if pitch is None:
pitch = p
if target_position is None:
target_position = t
self.sim.reset_debug_visualizer(distance, yaw, pitch, target_position)
def get_matrices(self, inverse=False):
"""
Return the view, projection, and viewport matrices.
Args:
inverse (bool): if True, it will also compute the inverse of the view, projection, and viewport matrices.
Returns:
if inverse:
np.array[4,4]: view matrix
np.array[4,4]: projection matrix
np.array[4,4]: viewport matrix
np.array[4,4]: inverse of the view matrix
np.array[4,4]: inverse of the projection matrix
np.array[4,4]: inverse o the viewport matrix
else:
np.array[4,4]: view matrix
np.array[4,4]: projection matrix
np.array[4,4]: viewport matrix
"""
width, height, V, P = self.sim.get_debug_visualizer()[:4]
Vp = np.array([[width / 2., 0, 0, width / 2.],
[0, height / 2., 0, height / 2.],
[0, 0, 0.5, 0.5],
[0, 0, 0, 1]])
if inverse:
Vinv = np.linalg.inv(V)
Pinv = np.linalg.inv(P)
Vpinv = np.linalg.inv(Vp)
return V, P, Vp, Vinv, Pinv, Vpinv
return V, P, Vp
def get_vectors(self):
"""
Return the forward, up, and lateral vectors of the camera.
Returns:
np.float[3]: forward vector
np.float[3]: up vector
np.float[3]: lateral vector (=cross product between forward and up vectors)
"""
up_vector, forward_vector = self.sim.get_debug_visualizer()[4:6]
lateral_vector = np.cross(forward_vector, up_vector)
return forward_vector, up_vector, lateral_vector
def compute_view_matrix_from_ypr(self, target_position, distance, yaw, pitch, roll, up_axis_index):
"""Compute the view matrix from the yaw, pitch, and roll angles.
The view matrix is the 4x4 matrix that maps the world coordinates into the camera coordinates. Basically,
it applies a rotation and translation such that the world is in front of the camera. That is, instead
of turning the camera to capture what we want in the world, we keep the camera fixed and turn the world.
Args:
target_position (np.float[3]): target focus point in Cartesian world coordinates
distance (float): distance from eye to focus point
yaw (float): yaw angle in radians left/right around up-axis
pitch (float): pitch in radians up/down.
roll (float): roll in radians around forward vector
up_axis_index (int): either 1 for Y or 2 for Z axis up.
Returns:
np.float[4,4]: the view matrix
More info:
[1] http://www.codinglabs.net/article_world_view_projection_matrix.aspx
[2] http://www.thecodecrate.com/opengl-es/opengl-transformation-matrices/
"""
return self.sim.compute_view_matrix_from_ypr(self, target_position=target_position, distance=distance, yaw=yaw,
pitch=pitch, roll=roll, up_axis_index=up_axis_index)
def compute_view_matrix(self, eye_position, target_position, up_vector):
"""Compute the view matrix.
The view matrix is the 4x4 matrix that maps the world coordinates into the camera coordinates. Basically,
it applies a rotation and translation such that the world is in front of the camera. That is, instead
of turning the camera to capture what we want in the world, we keep the camera fixed and turn the world.
Args:
eye_position (np.float[3]): eye position in Cartesian world coordinates
target_position (np.float[3]): position of the target (focus) point in Cartesian world coordinates
up_vector (np.float[3]): up vector of the camera in Cartesian world coordinates
Returns:
np.float[4,4]: the view matrix
More info:
[1] http://www.codinglabs.net/article_world_view_projection_matrix.aspx
[2] http://www.thecodecrate.com/opengl-es/opengl-transformation-matrices/
"""
return self.sim.compute_view_matrix(eye_position, target_position, up_vector)
def set_yaw_pitch(self, yaw, pitch, radian=True):
"""
Set the yaw and pitch angles.
Args:
yaw (float): yaw angle.
pitch (float): pitch angle.
radian (bool): If the given pitch and yaw angles are in radian.
"""
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)
def add_yaw_pitch(self, dyaw, dpitch, radian=True):
"""
Add a small amount `dyaw` and `dpitch` to the camera's current yaw and pitch angles.
Args:
dyaw (float): small amount to add to the camera's current yaw angle
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:]
if radian:
dyaw, dpitch = np.rad2deg(dyaw), np.rad2deg(dpitch)
yaw += dyaw
pitch += dpitch
self.sim.reset_debug_visualizer(dist, yaw, pitch, target_pos)
def get_rgb_image(self):
"""
Return the captured RGB image.
Returns:
np.array[W,H,C]: RGB image (width, height, RGB channels)
"""
return self.get_rgba_image()[:, :, :3]
def get_rgba_image(self):
"""
Return the captured RGBA image. 'A' stands for alpha channel (for opacity/transparency)
Returns:
np.array[W,H,C]: RGBA image (width, height, RGBA channels)
"""
width, height, view_matrix, projection_matrix = self.sim.get_debug_visualizer()[:4]
img = np.array(self.sim.get_camera_image(width, height, view_matrix, projection_matrix)[2])
img = img.reshape(width, height, 4) # RGBA
return img
def get_depth_image(self):
"""
Return the depth image.
Returns:
np.array[W,H]: depth image (width, height)
"""
width, height, view_matrix, projection_matrix = self.sim.get_debug_visualizer()[:4]
img = np.array(self.sim.get_camera_image(width, height, view_matrix, projection_matrix)[3])
img = img.reshape(width, height)
return img
def get_rgbad_image(self, concatenate=True):
"""
Return the RGBA and depth images.
Args:
concatenate (bool): If True, it will concatenate the RGBA and depth images such that it has a shape of
(width, height, 5).
Returns:
if concatenate:
np.array[W,H,C]: RGBAD image (width, height, RGBAD channels)
else:
np.array[W,H,C]: RGBA image (width, height, RGBA channels)
np.array[W,H]: depth image (width, height)
"""
width, height, view_matrix, projection_matrix = self.sim.get_debug_visualizer()[:4]
rgba, depth = self.sim.get_camera_image(width, height, view_matrix, projection_matrix)[2:4]
rgba = np.array(rgba).reshape(width, height, 4)
depth = np.array(depth).reshape(width, height)
if concatenate:
return np.dstack((rgba, depth))
return rgba, depth
def screen_to_world(self, x_screen, Vp_inv=None, P_inv=None, V_inv=None):
"""
Return the corresponding coordinates in the Cartesian world space from the coordinates of a point
on the screen.
Args:
x_screen (np.float[4]): augmented vector coordinates of a point on the screen
Vp_inv (np.float[4,4], None): inverse of viewport matrix. If None, it will be computed.
P_inv (np.float[4,4], None): inverse of projection matrix. If None, it will be computed.
V_inv (np.float[4,4], None): inverse of view matrix. If None, it will be computed.
Returns:
np.float[4]: augmented vector coordinates of the corresponding point in the world
"""
if Vp_inv is None:
Vp_inv = self.Vp_inv
if P_inv is None:
P_inv = self.Pinv
if V_inv is None:
V_inv = self.Vinv
x_ndc = Vp_inv.dot(x_screen)
x_ndc[1] = -x_ndc[1] # invert y-axis
x_ndc[2] = -x_ndc[2] # invert z-axis
x_eye = P_inv.dot(x_ndc)
x_eye = x_eye / x_eye[3] # normalize
x_world = V_inv.dot(x_eye)
return x_world
def world_to_screen(self, x_world, V=None, P=None, Vp=None):
"""
Return the corresponding screen coordinates from a 3D point in the world.
Args:
x_world (float[4]): augmented vector coordinates of a point in the Cartesian world space
V (np.float[4,4], None): view matrix. If None, it will be computed.
P (np.float[4,4], None): projection matrix. If None, it will be computed.
Vp (np.float[4,4], None): viewport matrix. If None, it will be computed.
Returns:
np.float[4]: augmented vector coordinates of the corresponding point on the screen
"""
if V is None:
V = self.V
if P is None:
P = self.P
if Vp is None:
Vp = self.Vp
x_eye = V.dot(x_world)
x_clip = P.dot(x_eye)
x_ndc = x_clip / x_clip[3] # normalize
x_ndc[1] = -x_ndc[1] # invert y-axis (as y pointing upward in projection but should point downward in screen)
x_ndc[2] = -x_ndc[2] # invert z-axis (to get right-handed coord. system, -1=close and 1=far)
x_screen = Vp.dot(x_ndc) # for depth between 0(=close) and 1(=far)
return x_screen
#############
# Operators #
#############
def __repr__(self):
"""Return the representation string of the object."""
return self.__class__.__name__
# Tests
if __name__ == '__main__':
from itertools import count
import time
from pyrobolearn.simulators import BulletSim
# create simulator
sim = BulletSim()
# load floor
floor_id = sim.load_urdf('plane.urdf', use_fixed_base=True)
# create camera
camera = WorldCamera(sim)
# define variables
radius = 2
theta = 0
dtheta = 0.01
for t in count():
# get camera information and print them
position = camera.position
yaw = camera.yaw
pitch = camera.pitch
dist = camera.dist
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("##########\n")
# move camera
theta += dtheta
# camera.position = np.array([0, radius * np.sin(theta), radius * np.cos(theta)])
camera.position = np.array([radius * np.sin(theta), -radius * np.cos(theta), 2])
# step in the simulator
sim.step()
time.sleep(1./254)