[FEATURE] Add Sensor.draw_debug method.

[Milotrince]

Description

Adds draw_debug method to base Sensor and current sensor implementations.
Also refactor MPLPlotter -> MPLLinePlotter and have a more generic MPLPlotter that can be used for other kind of plots.

Related Issue

Splits #1742 into smaller PRs for manageability

Motivation and Context

Allow for easily visually debugging sensor data.

How Has This Been / Can This Be Tested?

see example scripts

Screenshots (if appropriate):

python examples/sensors/imu_franka.py

python examples/sensors/contact_force_go2.py --no-force

python examples/sensors/contact_force_go2.py --force

Checklist:

• I read the CONTRIBUTING document.
• I followed the Submitting Code Changes section of CONTRIBUTING document.
• I tagged the title correctly (including BUG FIX/FEATURE/MISC/BREAKING)
• I updated the documentation accordingly or no change is needed.
• I tested my changes and added instructions on how to test it for reviewers.

[Kashu7100]

@Milotrince
[BUG REPORT] python examples/sensors/imu_franka.py

Traceback (most recent call last):
  File "/Users/kashu/research/Genesis/examples/sensors/imu_franka.py", line 129, in <module>
    main()
  File "/Users/kashu/research/Genesis/examples/sensors/imu_franka.py", line 83, in main
    scene.build()
  File "/Users/kashu/research/Genesis/genesis/utils/misc.py", line 140, in wrapper
    return method(self, *args, **kwargs)
           ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/Users/kashu/research/Genesis/genesis/engine/scene.py", line 797, in build
    self._visualizer.build()
  File "/Users/kashu/research/Genesis/genesis/vis/visualizer.py", line 192, in build
    self.reset()
  File "/Users/kashu/research/Genesis/genesis/vis/visualizer.py", line 169, in reset
    self._viewer.update(auto_refresh=True)
  File "/Users/kashu/research/Genesis/genesis/vis/viewer.py", line 153, in update
    self.context.update()
  File "/Users/kashu/research/Genesis/genesis/vis/rasterizer_context.py", line 952, in update
    self.update_sensors()
  File "/Users/kashu/research/Genesis/genesis/vis/rasterizer_context.py", line 807, in update_sensors
    self.sim._sensor_manager.draw_debug(self)
  File "/Users/kashu/research/Genesis/genesis/sensors/sensor_manager.py", line 92, in draw_debug
    sensor._draw_debug(context)
  File "/Users/kashu/research/Genesis/genesis/sensors/imu.py", line 342, in _draw_debug
    data = self.read(envs_idx=env_idx)
           ^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/Users/kashu/research/Genesis/genesis/utils/misc.py", line 150, in wrapper
    return method(self, *args, **kwargs)
           ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/Users/kashu/research/Genesis/genesis/sensors/base_sensor.py", line 225, in read
    return self._get_formatted_data(self._manager.get_cloned_from_cache(self), envs_idx)
           ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/Users/kashu/research/Genesis/genesis/sensors/base_sensor.py", line 331, in _get_formatted_data_dict
    return dict(zip(self._return_format.keys(), self._get_return_values(tensor, envs_idx)))
                                                ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/Users/kashu/research/Genesis/genesis/sensors/base_sensor.py", line 315, in _get_return_values
    envs_idx = self._sanitize_envs_idx(envs_idx)
               ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/Users/kashu/research/Genesis/genesis/sensors/base_sensor.py", line 338, in _sanitize_envs_idx
    return self._manager._sim._scene._sanitize_envs_idx(envs_idx)
           ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/Users/kashu/research/Genesis/genesis/engine/scene.py", line 1350, in _sanitize_envs_idx
    gs.raise_exception("`envs_idx` is not supported for non-parallelized scene.")
  File "/Users/kashu/research/Genesis/genesis/utils/misc.py", line 42, in raise_exception
    raise gs.GenesisException(msg)
genesis.GenesisException: `envs_idx` is not supported for non-parallelized scene.

python examples/sensors/contact_force_go2.py --force

Traceback (most recent call last):
  File "/Users/kashu/research/Genesis/examples/sensors/contact_force_go2.py", line 94, in <module>
    main()
  File "/Users/kashu/research/Genesis/examples/sensors/contact_force_go2.py", line 84, in main
    scene.step()
  File "/Users/kashu/research/Genesis/genesis/utils/misc.py", line 150, in wrapper
    return method(self, *args, **kwargs)
           ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/Users/kashu/research/Genesis/genesis/engine/scene.py", line 925, in step
    self._visualizer.update(force=False, auto=refresh_visualizer)
  File "/Users/kashu/research/Genesis/genesis/vis/visualizer.py", line 199, in update
    self._viewer.update(auto_refresh=auto)
  File "/Users/kashu/research/Genesis/genesis/vis/viewer.py", line 153, in update
    self.context.update()
  File "/Users/kashu/research/Genesis/genesis/vis/rasterizer_context.py", line 952, in update
    self.update_sensors()
  File "/Users/kashu/research/Genesis/genesis/vis/rasterizer_context.py", line 807, in update_sensors
    self.sim._sensor_manager.draw_debug(self)
  File "/Users/kashu/research/Genesis/genesis/sensors/sensor_manager.py", line 92, in draw_debug
    sensor._draw_debug(context)
  File "/Users/kashu/research/Genesis/genesis/sensors/contact_force.py", line 377, in _draw_debug
    self.debug_object = context.draw_debug_arrow(pos=pos[0], vec=vec[0], color=self._options.debug_color)
                        ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/Users/kashu/research/Genesis/genesis/vis/rasterizer_context.py", line 829, in draw_debug_arrow
    gu.z_up_to_R(tensor_to_array(vec).astype(np.float32), out=pose[0, :3, :3])
  File "/Users/kashu/research/Genesis/genesis/utils/geom.py", line 1290, in z_up_to_R
    return _np_z_up_to_R(z, up, out)
           ^^^^^^^^^^^^^^^^^^^^^^^^^
  File "/Users/kashu/research/Genesis/.venv/lib/python3.12/site-packages/numba/np/arrayobj.py", line 2134, in normalize_reshape_value
    raise ValueError("total size of new array must be unchanged")
ValueError: total size of new array must be unchanged

[Genesis] [01:04:50] [ERROR] ValueError: total size of new array must be unchanged

[Kashu7100]

Also the marker seems off? This is on Mac M4.

[Milotrince]

@Kashu7100

[ERROR] ValueError: total size of new array must be unchanged

I'm aware of this. I have a local fix, will push soon to the next PR #1772

[duburcqa]

Thank you for the report @Kashu7100 Some unit tests are missing and will be added!