From 8b550c33c332e54cc993c27e9aaf8259816a8035 Mon Sep 17 00:00:00 2001
From: Rooholla-KhorramBakht <learn.kho@gmail.com>
Date: Mon, 18 Mar 2024 16:38:31 -0400
Subject: [PATCH] a flag added to the interface classes to indicate if it's
 simulated or real

---
 Go2Py/robot/interface/dds.py  |  2 +-
 Go2Py/robot/interface/ros2.py |  1 +
 Go2Py/sim/mujoco.py           |  1 +
 examples/fsm.ipynb            | 16 +++++++++-------
 4 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/Go2Py/robot/interface/dds.py b/Go2Py/robot/interface/dds.py
index 6c5a45d..d438e5a 100644
--- a/Go2Py/robot/interface/dds.py
+++ b/Go2Py/robot/interface/dds.py
@@ -29,7 +29,7 @@ class GO2Real():
         self.mode = mode
         if self.mode == 'highlevel':
             raise NotImplementedError('DDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface.')
-
+        self.simulated = False
         self.highcmd_topic_name = "rt/go2/twist_cmd"
         self.lowcmd_topic_name = "rt/go2/lowcmd"
         self.lowstate_topic_name = "rt/lowstate"
diff --git a/Go2Py/robot/interface/ros2.py b/Go2Py/robot/interface/ros2.py
index 190b756..e9d5dad 100644
--- a/Go2Py/robot/interface/ros2.py
+++ b/Go2Py/robot/interface/ros2.py
@@ -66,6 +66,7 @@ class GO2Real(Node):
         ωz_max=0.5,
     ):
         assert mode in ['highlevel', 'lowlevel'], "mode should be either 'highlevel' or 'lowlevel'"
+        self.simulated = False
         self.mode = mode
         self.node_name = "go2py_highlevel_subscriber"
         self.highcmd_topic = "/go2/twist_cmd"
diff --git a/Go2Py/sim/mujoco.py b/Go2Py/sim/mujoco.py
index 11bacd6..9c18050 100644
--- a/Go2Py/sim/mujoco.py
+++ b/Go2Py/sim/mujoco.py
@@ -15,6 +15,7 @@ class Go2Sim:
         self.model = mujoco.MjModel.from_xml_path(
             os.path.join(ASSETS_PATH, 'mujoco/go2.xml')
         )
+        self.simulated = True
         self.data = mujoco.MjData(self.model)
         self.dt = dt
         _render_dt = 1/60
diff --git a/examples/fsm.ipynb b/examples/fsm.ipynb
index 62cb9a7..94fefa1 100644
--- a/examples/fsm.ipynb
+++ b/examples/fsm.ipynb
@@ -184,9 +184,10 @@
     "        self.running = True\n",
     "        self.fsm_thread = threading.Thread(target = self.update)\n",
     "        self.fsm_thread.start()\n",
-    "\n",
-    "        self.sim_thread = threading.Thread(target=self.simUpdate)\n",
-    "        self.sim_thread.start()\n",
+    "        # if the robot is a simulation, create a thread for stepping it\n",
+    "        if robot.simulated:\n",
+    "            self.sim_thread = threading.Thread(target=self.simUpdate)\n",
+    "            self.sim_thread.start()\n",
     "\n",
     "    def setMode(self, mode):\n",
     "        assert mode in self.modes.keys(), 'the requested control update mode is not implemented'\n",
@@ -228,6 +229,10 @@
     "            time.sleep(self.dT)\n",
     "            self.updateCommands()\n",
     "    \n",
+    "    def close(self):\n",
+    "        self.running = False\n",
+    "\n",
+    "    # The following functions each are the states of the FSM\n",
     "    def damping(self):\n",
     "        # print('damping')\n",
     "        if self.remote.standUpDownSeq():\n",
@@ -288,10 +293,7 @@
     "    def switch_back_to_locked_stance(self):\n",
     "        if time.time()-self.timer > 0.5:\n",
     "            # print(\"going back to locked stance\")\n",
-    "            self.state = \"locked_stance\"\n",
-    "            \n",
-    "    def close(self):\n",
-    "        self.running = False"
+    "            self.state = \"locked_stance\""
    ]
   },
   {