fix(sim): convergence detection

CMakeLists.txt

@@ -67,7 +67,7 @@ if (${INCLUDE_UTN_MODELS})
     if (DEFINED GITLAB_MODELS_TOKEN)
       include(download_scenes)
       include(compile_scenes)
-      download_scenes(fc45f45c937e91a7e351a1dab30e4cd881280718)
+      download_scenes(3fbaa094c35381287d4f6bf3259aae7f123a3d94)
       compile_scenes()
     else()
       message(FATAL_ERROR "GITLAB_MODELS_TOKEN missing. Not including lab scene")

python/rcsss/envs/sim.py

@@ -58,8 +58,9 @@ def __init__(self, env: gym.Env, simulation: sim.Sim, collision_env: FR3Sim, che
         self.check_home_collision = check_home_collision
 
     def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], SupportsFloat, bool, bool, dict[str, Any]]:
+        # TODO: we should set the state of the sim to the state of the real robot
         _, _, _, _, info = self.collision_env.step(action)
-        if info["collision"] or not info["ik_success"]:
+        if info["collision"] or not info["ik_success"] or not info["is_sim_converged"]:
             # return old obs, with truncated and print warning
             self._logger.warning("Collision detected! Truncating episode.")
             if self.last_obs is None:

src/sim/FR3.h

@@ -19,7 +19,8 @@ const common::Vector7d q_home((common::Vector7d() << 0, -M_PI_4, 0, -3 * M_PI_4,
 
 struct FR3Config : common::RConfig {
   rcs::common::Pose tcp_offset = rcs::common::Pose::Identity();
-  double joint_rotational_tolerance = .5 * (std::numbers::pi / 180.0);
+  double joint_rotational_tolerance =
+      .05 * (std::numbers::pi / 180.0);      // 0.05 degree
   double seconds_between_callbacks = 0.1;    // 10 Hz
   size_t ik_duration_in_milliseconds = 300;  // milliseconds
   bool realtime = false;

src/sim/sim.cpp

@@ -5,6 +5,7 @@
 
 #include <chrono>
 #include <functional>
+#include <iostream>
 #include <thread>
 
 namespace rcs {
@@ -103,6 +104,9 @@ void Sim::step_until_convergence() {
     this->convergence_steps++;
     this->converged = invoke_condition_callbacks();
   };
+  if (this->convergence_steps == this->cfg.max_convergence_steps) {
+    std::cerr << "WARNING: Max convergence steps reached!" << std::endl;
+  }
 }
 
 void Sim::step(size_t k) {

src/sim/sim.h

@@ -27,7 +27,7 @@ class Renderer {
 struct Config {
   bool async = false;
   bool realtime = false;
-  int max_convergence_steps = -1;
+  int max_convergence_steps = 5000;
 };
 
 struct Callback {