[BUG FIX] Fix camera following entity for 'fix_orientation=True'. (#2038)

* Fix camera following entity for 'fix_orientation=True'.
* Add follow enity example script.

Author: duburcqa

examples/rendering/follow_entity.py

@@ -0,0 +1,49 @@
+import argparse
+
+import genesis as gs
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("-f", "--fix", action="store_true", default=False)
+    args = parser.parse_args()
+
+    gs.init()
+
+    scene = gs.Scene(
+        vis_options=gs.options.VisOptions(
+            rendered_envs_idx=(0,),
+        ),
+        profiling_options=gs.options.ProfilingOptions(
+            show_FPS=False,
+        ),
+        show_viewer=False,
+    )
+    scene.add_entity(morph=gs.morphs.Plane())
+    cube = scene.add_entity(
+        gs.morphs.Box(
+            size=(0.1, 0.1, 0.1),
+            pos=(0.0, -0.9, 1.0),
+            euler=(15.0, 30.0, 60.0),
+        )
+    )
+
+    cam = scene.add_camera(
+        res=(640, 480),
+        pos=(2.0, 0.0, 1.5),
+        lookat=(0, 0, 0.7),
+        fov=40,
+        GUI=True,
+    )
+    cam.follow_entity(cube, fix_orientation=args.fix)
+
+    scene.build()
+
+    cube.set_dofs_velocity([0.0, 5.0, 0.0, 0.0, 0.0, 1.0])
+    for _ in range(100):
+        scene.step()
+        cam.render()
+
+
+if __name__ == "__main__":
+    main()

genesis/vis/camera.py

@@ -296,6 +296,9 @@ def follow_entity(self, entity, fixed_axis=(None, None, None), smoothing=None, f
                     # Note that it is not possible to expand / tile because the batch size is unknown before build.
                     pos_rel = pos_rel.unsqueeze(0)
 
+        if (pos_rel.abs() < gs.EPS).all():
+            gs.raise_exception("Camera must not be co-located with base link of entity to which it is attached.")
+
         self._followed_entity = entity
         self._follow_pos_rel = pos_rel
         self._follow_fixed_axis = fixed_axis
@@ -346,8 +349,8 @@ def update_following(self):
                 camera_lookat[:] = self._follow_smoothing * camera_lookat + (1.0 - self._follow_smoothing) * entity_pos
         else:
             camera_pos[:] = entity_pos
-            camera_lookat[:] = entity_pos
-
+            if not self._follow_fix_orientation:
+                camera_lookat[:] = entity_pos
         camera_pos += follow_pos_rel
 
         # Fix the camera's position along the specified axis if requested