Skip to content

Commit

Permalink
[world] Fixed camera control for the BulletWorld
Browse files Browse the repository at this point in the history
  • Loading branch information
@Tigul
Tigul committed Apr 9, 2024
1 parent e414c19 commit 5e1a3f1
Show file tree
Hide file tree
Showing 2 changed files with 40 additions and 16 deletions.
15 changes: 13 additions & 2 deletions src/pycram/world.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -156,6 +156,11 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ

self.cache_manager = CacheManager(self.cache_dir, self.data_directory)

self.id: Optional[int] = -1
# This is used to connect to the physics server (allows multiple clients)

self._init_world(mode)

self.is_prospection_world: bool = is_prospection_world
self._init_and_sync_prospection_world()

Expand All @@ -165,8 +170,7 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ
self.objects: List[Object] = []
# List of all Objects in the World

self.id: Optional[int] = -1
# This is used to connect to the physics server (allows multiple clients)


self.mode: WorldMode = mode
# The mode of the simulation, can be "GUI" or "DIRECT"
Expand All @@ -177,6 +181,13 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool, simulation_frequ

self._current_state: Optional[WorldState] = None

@abstractmethod
def _init_world(self, mode: WorldMode):
"""
Initializes the physics simulation.
"""
raise NotImplementedError

def _init_events(self):
"""
Initializes dynamic events that can be used to react to changes in the World.
Expand Down
41 changes: 27 additions & 14 deletions src/pycram/worlds/bullet_world.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,9 +48,6 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo
"""
super().__init__(mode=mode, is_prospection_world=is_prospection_world, simulation_frequency=sim_frequency)

self._gui_thread: Gui = Gui(self, mode)
self._gui_thread.start()

# This disables file caching from PyBullet, since this would also cache
# files that can not be loaded
p.setPhysicsEngineParameter(enableFileCaching=0)
Expand All @@ -66,6 +63,11 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo
_ = Object("floor", ObjectType.ENVIRONMENT, "plane" + self.extension,
world=self)

def _init_world(self, mode: WorldMode):
self._gui_thread: Gui = Gui(self, mode)
self._gui_thread.start()
time.sleep(0.1)

def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None) -> int:
if pose is None:
pose = Pose()
Expand Down Expand Up @@ -342,6 +344,11 @@ def run(self):
else:
self.world.id = p.connect(p.GUI)

# DISCLAIMER
# This camera control only works if the WorldMooe.GUI BulletWorld is the first one to be created. This is
# due to a bug in the function pybullet.getDebugVisualizerCamera() which only returns the information of
# the first created simulation.

# Disable the side windows of the GUI
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0, physicsClientId=self.world.id)
# Change the init camera pose
Expand Down Expand Up @@ -378,18 +385,19 @@ def run(self):
# Loop to update the camera position based on keyboard events
while p.isConnected(self.world.id):
# Monitor user input
keys = p.getKeyboardEvents()
mouse = p.getMouseEvents()
keys = p.getKeyboardEvents(self.world.id)
mouse = p.getMouseEvents(self.world.id)

# Get infos about the camera
width, height, dist = (p.getDebugVisualizerCamera()[0],
p.getDebugVisualizerCamera()[1],
p.getDebugVisualizerCamera()[10])
camera_target_position = p.getDebugVisualizerCamera()[11]
#print("width: ", width, "height: ", height, "dist: ", dist)
camera_target_position = p.getDebugVisualizerCamera(self.world.id)[11]

# Get vectors used for movement on x,y,z Vector
x_vec = [p.getDebugVisualizerCamera()[2][i] for i in [0, 4, 8]]
y_vec = [p.getDebugVisualizerCamera()[2][i] for i in [2, 6, 10]]
x_vec = [p.getDebugVisualizerCamera(self.world.id)[2][i] for i in [0, 4, 8]]
y_vec = [p.getDebugVisualizerCamera(self.world.id)[2][i] for i in [2, 6, 10]]
z_vec = (0, 0, 1) # [p.getDebugVisualizerCamera()[2][i] for i in [1, 5, 9]]

# Check the mouse state
Expand All @@ -411,10 +419,10 @@ def run(self):
mouse_state[2] = m[4]

# change visibility by clicking the mousewheel
if m[4] == 6 and m[3] == 1 and visible == 1:
visible = 0
elif m[4] == 6 and visible == 0:
visible = 1
# if m[4] == 6 and m[3] == 1 and visible == 1:
# visible = 0
# elif m[4] == 6 and visible == 0:
# visible = 1

# camera movement when the left mouse button is pressed
if mouse_state[0] == 3:
Expand All @@ -436,6 +444,7 @@ def run(self):
elif mouse_y > old_mouse_y:
camera_yaw -= (speed_y / 4) + 1

# Camera movement when the middle mouse button is pressed
if mouse_state[1] == 3:
speed_x = abs(old_mouse_x - mouse_x)
factor = 0.05
Expand Down Expand Up @@ -529,10 +538,14 @@ def run(self):
dist -= dist * 0.02 * speed_mult
elif ord("-") in keys:
dist += dist * 0.02 * speed_mult
# print("dist: ", dist)
# print("camera_yaw: ", camera_yaw)
# print("camera_pitch: ", camera_pitch)
# print("camera_target_position: ", camera_target_position)

p.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=camera_yaw, cameraPitch=camera_pitch,
cameraTargetPosition=camera_target_position)
cameraTargetPosition=camera_target_position, physicsClientId=self.world.id)
if visible == 0:
camera_target_position = (0.0, -50, 50)
p.resetBasePositionAndOrientation(sphere_uid, camera_target_position, [0, 0, 0, 1])
p.resetBasePositionAndOrientation(sphere_uid, camera_target_position, [0, 0, 0, 1], physicsClientId=self.world.id)
time.sleep(1. / 80.)

0 comments on commit 5e1a3f1

Please sign in to comment.