From c4bd73c2feeb4771dbc0ba97d649c398a114d862 Mon Sep 17 00:00:00 2001 From: LuckyMod <62712260+Likhithsai2580@users.noreply.github.com> Date: Mon, 13 Jan 2025 08:39:39 +0530 Subject: [PATCH] Add camera follow entity feature Fixes #538 Add functionality for the camera to follow an entity. * **Camera Class (`genesis/vis/camera.py`)** - Add `_follow_entity`, `_follow_height`, and `_follow_smoothing` attributes. - Add `follow_entity` method to set the camera to follow a specified entity. - Update `render` method to call `follow_entity` if enabled. * **Scene Class (`genesis/engine/scene.py`)** - Add `set_camera_follow_entity` method to set the camera to follow a specified entity. - Update `step` method to call `set_camera_follow_entity` if enabled. * **Drone Example (`examples/drone/interactive_drone.py`)** - Add `--follow` argument to enable camera follow. - Update `main` function to use `follow_entity` method if `--follow` argument is provided. --- examples/drone/interactive_drone.py | 11 +++++++ genesis/engine/scene.py | 22 ++++++++++++++ genesis/vis/camera.py | 47 +++++++++++++++++++++++++++++ 3 files changed, 80 insertions(+) diff --git a/examples/drone/interactive_drone.py b/examples/drone/interactive_drone.py index 6a0e0788..47a39123 100644 --- a/examples/drone/interactive_drone.py +++ b/examples/drone/interactive_drone.py @@ -134,6 +134,7 @@ def main(): parser = argparse.ArgumentParser() parser.add_argument("-v", "--vis", action="store_true", default=True, help="Enable visualization (default: True)") parser.add_argument("-m", "--mac", action="store_true", default=False, help="Running on MacOS (default: False)") + parser.add_argument("-f", "--follow", action="store_true", default=False, help="Enable camera follow (default: False)") args = parser.parse_args() # Initialize Genesis @@ -184,6 +185,16 @@ def main(): listener = keyboard.Listener(on_press=controller.on_press, on_release=controller.on_release) listener.start() + if args.follow: + camera = scene.add_camera( + res=(640, 480), + pos=(0.0, -4.0, 2.0), + lookat=(0.0, 0.0, 0.5), + fov=45, + GUI=True, + ) + scene.set_camera_follow_entity(camera, drone, height=2.0, smoothing=0.1) + if args.mac: # Run simulation in another thread sim_thread = threading.Thread(target=run_sim, args=(scene, drone, controller)) diff --git a/genesis/engine/scene.py b/genesis/engine/scene.py index 5fcadfbe..00bd2242 100644 --- a/genesis/engine/scene.py +++ b/genesis/engine/scene.py @@ -729,6 +729,10 @@ def step(self, update_visualizer=True): if self._show_FPS: self.FPS_tracker.step() + # Update camera to follow entity if enabled + if hasattr(self, "_camera_follow_entity") and self._camera_follow_entity is not None: + self._camera_follow_entity.follow_entity() + def _step_grad(self): self._sim.collect_output_grads() self._sim._step_grad() @@ -913,6 +917,24 @@ def _backward(self): self._backward_ready = False self._forward_ready = False + def set_camera_follow_entity(self, camera, entity, height=None, smoothing=None): + """ + Set the camera to follow a specified entity. + + Parameters + ---------- + camera : genesis.Camera + The camera to follow the entity. + entity : genesis.Entity + The entity to follow. + height : float, optional + The height at which the camera should follow the entity. If None, the camera will maintain its current height. + smoothing : float, optional + The smoothing factor for the camera's movement. If None, no smoothing will be applied. + """ + camera.follow_entity(entity, height, smoothing) + self._camera_follow_entity = camera + # ------------------------------------------------------------------------------------ # ----------------------------------- properties ------------------------------------- # ------------------------------------------------------------------------------------ diff --git a/genesis/vis/camera.py b/genesis/vis/camera.py index 687ab052..2eaa4ce1 100644 --- a/genesis/vis/camera.py +++ b/genesis/vis/camera.py @@ -93,6 +93,10 @@ def __init__( self._in_recording = False self._recorded_imgs = [] + self._follow_entity = None + self._follow_height = None + self._follow_smoothing = None + if self._model not in ["pinhole", "thinlens"]: gs.raise_exception(f"Invalid camera model: {self._model}") @@ -146,6 +150,9 @@ def render(self, rgb=True, depth=False, segmentation=False, colorize_seg=False, rgb_arr, depth_arr, seg_idxc_arr, seg_arr, normal_arr = None, None, None, None, None + if self._follow_entity is not None: + self.follow_entity() + if self._raytracer is not None: if rgb: self._raytracer.update_scene() @@ -279,6 +286,46 @@ def set_params(self, fov=None, aperture=None, focus_dist=None): if self._raytracer is not None: self._raytracer.update_camera(self) + @gs.assert_built + def follow_entity(self, entity, height=None, smoothing=None): + """ + Set the camera to follow a specified entity. + + Parameters + ---------- + entity : genesis.Entity + The entity to follow. + height : float, optional + The height at which the camera should follow the entity. If None, the camera will maintain its current height. + smoothing : float, optional + The smoothing factor for the camera's movement. If None, no smoothing will be applied. + """ + self._follow_entity = entity + self._follow_height = height + self._follow_smoothing = smoothing + + def follow_entity(self): + """ + Update the camera position to follow the specified entity. + """ + if self._follow_entity is None: + return + + entity_pos = self._follow_entity.get_pos() + camera_pos = np.array(self._pos) + + if self._follow_height is not None: + camera_pos[2] = self._follow_height + + if self._follow_smoothing is not None: + camera_pos[:2] = ( + self._follow_smoothing * camera_pos[:2] + (1 - self._follow_smoothing) * entity_pos[:2] + ) + else: + camera_pos[:2] = entity_pos[:2] + + self.set_pose(pos=camera_pos, lookat=entity_pos) + @gs.assert_built def start_recording(self): """