From 7c14a8215cc5a2b6d9a1267e2b7657699b3dd078 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20J=C3=BClg?= <tobias.juelg@utn.de>
Date: Fri, 9 Aug 2024 11:38:57 +0200
Subject: [PATCH] fix(sim): convergence detection

- set default max convergence steps to 5000
- added warning message if max convergence steps are reached
- decreased the joint rotational tolerance by a factor of 10 to 0.05 degree
- added sim convergence as criterion for collision guard
- updated models repository version to include the new kp and kv values
---
 CMakeLists.txt           | 2 +-
 python/rcsss/envs/sim.py | 3 ++-
 src/sim/FR3.h            | 3 ++-
 src/sim/sim.cpp          | 4 ++++
 src/sim/sim.h            | 2 +-
 5 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index ad284e00..d24fd7e7 100644
--- a/CMakeLists.txt
+++ b/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")
diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py
index 4e21eb5d..ca6d332b 100644
--- a/python/rcsss/envs/sim.py
+++ b/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:
diff --git a/src/sim/FR3.h b/src/sim/FR3.h
index e90cd3ec..5afb3cb7 100644
--- a/src/sim/FR3.h
+++ b/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;
diff --git a/src/sim/sim.cpp b/src/sim/sim.cpp
index 5de9623d..99df5147 100644
--- a/src/sim/sim.cpp
+++ b/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) {
diff --git a/src/sim/sim.h b/src/sim/sim.h
index f8c2be16..c2b6e5a0 100644
--- a/src/sim/sim.h
+++ b/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 {