From ea31592e97a72dec38b5feb78262404f805ae6b1 Mon Sep 17 00:00:00 2001
From: fernandopalafox <fernandopalafox@utexas.edu>
Date: Fri, 5 Apr 2024 12:04:29 -0500
Subject: [PATCH] script to test control extraction

---
 scripts/test_controller.py | 36 ++++++++++++++++++++++++++++++++++++
 1 file changed, 36 insertions(+)
 create mode 100644 scripts/test_controller.py

diff --git a/scripts/test_controller.py b/scripts/test_controller.py
new file mode 100644
index 0000000..58a3a26
--- /dev/null
+++ b/scripts/test_controller.py
@@ -0,0 +1,36 @@
+import robosuite as suite
+from robosuite.controllers import load_controller_config
+import numpy as np
+
+# Load the desired controller's default config as a dict
+config = load_controller_config(default_controller="OSC_POSE")
+env_config = {}
+env_config["env_name"] = "Lift"
+env_config["robots"] = "Panda"
+env_config["camera_names"] = ["frontview"]
+env_config["camera_heights"] = 480
+env_config["camera_widths"] = 480
+env_config["control_freq"] = 10
+env_config["controller_configs"] = suite.load_controller_config(default_controller="OSC_POSE")
+env_config["has_renderer"] = False
+env_config["has_offscreen_renderer"] = False
+env_config["ignore_done"] = True
+env_config["use_camera_obs"] = False
+
+# Make the environment
+env = suite.make(**env_config)
+
+# Reset the environment
+env.reset()
+
+# Get action limits
+low, high = env.action_spec
+
+# do visualization
+for i in range(10000):
+    action = np.random.uniform(low, high)
+    obs, reward, done, _ = env.step(action)
+
+    # Print action and corresponding torque from the environment. Format them so it's clear
+    print(f"Action: {action}")
+    print(f"Torque: {env.sim.data.ctrl}")
\ No newline at end of file