Merge pull request #171 from utn-mi/juelg/on-demand-camera-render

feat(sim camera): on demand camera render

Author: juelg

README.md

@@ -1,4 +1,5 @@
 # Robot Control Stack
+RCS is a unified and multilayered robot control interface over a MuJoCo simulation and real world robot currently implemented for the FR3.
 ## Requirements
 We build and test RCS on the latest Debian and on the latest Ubuntu LTS.
 
@@ -29,17 +30,27 @@ Import the library in python:
 ```python
 import rcsss
 ```
-The package includes a command line interface which define useful commands to handle the hardware robot.
+Checkout the python examples that we provide in [python/examples](python/examples):
+- [fr3.py](python/examples/fr3.py) shows direct robot control with RCS's python bindings
+- [env_joint_control.py](python/examples/env_joint_control.py) and [env_cartesian_control.py](python/examples/env_cartesian_control.py) demonstrates RCS's high level [gymnasium](https://gymnasium.farama.org/) interface both for joint- and end effector space control
+All of these examples work both in the MuJoCo simulation as well as on your hardware FR3.
+Just switch between the following settings in the example script
+```python
+ROBOT_INSTANCE = RobotInstance.SIMULATION
+# ROBOT_INSTANCE = RobotInstance.HARDWARE
+```
+and add your robot credentials to a `.env` file like this:
+```env
+DESK_USERNAME=...
+DESK_PASSWORD=...
+```
+
+### Command Line Interface
+The package includes a command line interface which define useful commands to handle the FR3 robot without the need to use the Desk Website.
 To list all available subcommands use:
 ```shell
 python -m rcsss --help
 ```
-A sample config can be generated via the following CLI command.
-```shell
-python -m rcsss sample-config
-```
-The command will produce a `config.yaml` file with sample values.
-See [config.py](python/rcsss/config.py) for a description of the config fields.
 
 ## Development
 ```shell

python/rcsss/_core/sim.pyi

@@ -159,11 +159,18 @@ class SimCameraSet:
 
 class SimCameraSetConfig:
     cameras: dict[str, SimCameraConfig]
-    frame_rate: int
     max_buffer_frames: int
     resolution_height: int
     resolution_width: int
     def __init__(self) -> None: ...
+    @property
+    def frame_rate(self) -> int:
+        """
+        The frame rate in which the cameras render in Hz. If set to zero, the camera frames will render on demand and without fixed rate which takes away compute effort.
+        """
+
+    @frame_rate.setter
+    def frame_rate(self, arg0: int) -> None: ...
 
 def open_gui_window(uuid: str) -> None: ...
 

python/rcsss/config.py

@@ -241,7 +241,7 @@ def __call__(  # type: ignore
         render_mode: str = "human",
         control_mode: ControlMode = ControlMode.CARTESIAN_TRPY,
         resolution: tuple[int, int] | None = None,
-        frame_rate: int = 10,
+        frame_rate: int = 0,
         delta_actions: bool = True,
     ) -> gym.Env:
         if resolution is None:
@@ -272,7 +272,7 @@ def __call__(  # type: ignore
         render_mode: str = "human",
         control_mode: ControlMode = ControlMode.CARTESIAN_TRPY,
         resolution: tuple[int, int] | None = None,
-        frame_rate: int = 10,
+        frame_rate: int = 0,
         delta_actions: bool = True,
     ) -> gym.Env:
         if resolution is None:
@@ -319,7 +319,7 @@ def __call__(  # type: ignore
         render_mode: str = "human",
         control_mode: ControlMode = ControlMode.CARTESIAN_TRPY,
         resolution: tuple[int, int] | None = None,
-        frame_rate: int = 10,
+        frame_rate: int = 0,
         delta_actions: bool = True,
     ) -> gym.Env:
         if resolution is None:

python/rcsss/envs/factories.py

@@ -486,7 +486,10 @@ PYBIND11_MODULE(_core, m) {
   py::class_<rcs::sim::SimCameraSetConfig>(sim, "SimCameraSetConfig")
       .def(py::init<>())
       .def_readwrite("cameras", &rcs::sim::SimCameraSetConfig::cameras)
-      .def_readwrite("frame_rate", &rcs::sim::SimCameraSetConfig::frame_rate)
+      .def_readwrite("frame_rate", &rcs::sim::SimCameraSetConfig::frame_rate,
+                     "The frame rate in which the cameras render in Hz. If set "
+                     "to zero, the camera frames will render on demand and "
+                     "without fixed rate which takes away compute effort.")
       .def_readwrite("resolution_width",
                      &rcs::sim::SimCameraSetConfig::resolution_width)
       .def_readwrite("resolution_height",

src/pybind/rcsss.cpp

@@ -24,10 +24,13 @@ namespace sim {
 SimCameraSet::SimCameraSet(std::shared_ptr<Sim> sim, SimCameraSetConfig cfg)
     : sim{sim}, cfg{cfg}, buffer{}, buffer_lock{}, cameras{} {
   for (auto const& [id, cam] : cfg.cameras) {
+    // if frame_rate is zero, then we only render when an image is requested
+    // this mode is useful when no videos are required as it speeds up the
+    // simulation significantly
     this->sim->register_rendering_callback(
         [this](const std::string& id, mjrContext& ctx, mjvScene& scene,
                mjvOption& opt) { this->frame_callback(id, ctx, scene, opt); },
-        id, 1.0 / this->cfg.frame_rate, this->cfg.resolution_width,
+        id, this->cfg.frame_rate, this->cfg.resolution_width,
         this->cfg.resolution_height);
 
     mjvCamera mjcam;
@@ -57,6 +60,9 @@ void SimCameraSet::clear_buffer() {
 }
 
 std::optional<FrameSet> SimCameraSet::get_latest_frameset() {
+  if (this->cfg.frame_rate == 0) {
+    this->render_all();
+  }
   if (buffer.empty()) {
     return std::nullopt;
   }
@@ -73,6 +79,14 @@ std::optional<FrameSet> SimCameraSet::get_timestamp_frameset(float ts) {
   return std::nullopt;
 }
 
+void SimCameraSet::render_all() {
+  for (auto const& [id, cam] : this->cfg.cameras) {
+    mjrContext* ctx = this->sim->renderer.get_context(id);
+    this->frame_callback(id, *ctx, this->sim->renderer.scene,
+                         this->sim->renderer.opt);
+  }
+}
+
 void SimCameraSet::frame_callback(const std::string& id, mjrContext& ctx,
                                   mjvScene& scene, mjvOption& opt) {
   mjrRect viewport = mjr_maxViewport(&ctx);

src/sim/camera.cpp

@@ -66,6 +66,7 @@ class SimCameraSet {
   std::unordered_map<std::string, mjvCamera> cameras;
   std::mutex buffer_lock;
   mjtNum last_ts = 0;
+  void render_all();
 };
 }  // namespace sim
 }  // namespace rcs

src/sim/camera.h

@@ -65,7 +65,8 @@ void GuiClient::render_loop() {
   mjv_defaultFreeCamera(this->m, &this->cam);
   mjv_defaultOption(&this->opt);
   mjv_defaultScene(&this->scn);
-  mjv_makeScene(this->m, &this->scn, mjFONTSCALE_100);
+  size_t max_geoms = 2000;
+  mjv_makeScene(this->m, &this->scn, max_geoms);
   mjrRect viewport = {0, 0, 0, 0};
   this->platform_ui->RefreshMjrContext(this->m, mjFONTSCALE_100);
   this->platform_ui->SetVSync(true);

src/sim/gui_client.cpp

@@ -164,17 +164,19 @@ void Sim::register_all_cb(std::function<bool(void)> cb,
 void Sim::register_rendering_callback(
     std::function<void(const std::string&, mjrContext&, mjvScene&, mjvOption&)>
         cb,
-    const std::string& id, mjtNum seconds_between_calls, size_t width,
-    size_t height) {
+    const std::string& id, int frame_rate, size_t width, size_t height) {
   this->renderer.register_context(id, width, height);
-  // dont register off screen in normal callback, but special gui callback
-  this->rendering_callbacks.push_back(
-      RenderingCallback{.cb = cb,
-                        .id = id,
-                        .seconds_between_calls = seconds_between_calls,
-                        // this is negative so that we will directly render the
-                        // cameras in the first step
-                        .last_call_timestamp = -seconds_between_calls});
+  // in case frame_rate is zero, rendering needs to be triggered
+  // manually
+  if (frame_rate != 0) {
+    this->rendering_callbacks.push_back(
+        RenderingCallback{.cb = cb,
+                          .id = id,
+                          .seconds_between_calls = 1.0 / frame_rate,
+                          // this is negative so that we will directly render
+                          // the cameras in the first step
+                          .last_call_timestamp = -1.0 / frame_rate});
+  }
 }
 
 void Sim::start_gui_server(const std::string& id) {

src/sim/sim.cpp

@@ -98,8 +98,7 @@ class Sim {
       std::function<void(const std::string& id, mjrContext&, mjvScene&,
                          mjvOption&)>
           cb,
-      const std::string& id, mjtNum seconds_between_calls, size_t width,
-      size_t height);
+      const std::string& id, int frame_rate, size_t width, size_t height);
   void start_gui_server(const std::string& id);
   void stop_gui_server();
 };