From 89a0316fb4e032fe879f7e6d2bfef9d83072dfaf Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Fri, 10 Oct 2025 18:21:06 +0200
Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20Adds=20script=20to=20play=20with=20?=
 =?UTF-8?q?kinematics?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 simulation/play_kinematics.py | 45 +++++++++++++++++++++++++++++++++++
 1 file changed, 45 insertions(+)
 create mode 100644 simulation/play_kinematics.py

diff --git a/simulation/play_kinematics.py b/simulation/play_kinematics.py
new file mode 100644
index 0000000..fcd0dc5
--- /dev/null
+++ b/simulation/play_kinematics.py
@@ -0,0 +1,45 @@
+import time
+import numpy as np
+import pybullet as p
+
+from src.robot.kinematics import Kinematics, BodyStateT, KinConfig
+from src.robot.gait import GaitController, GaitStateT, GaitType, default_offset, default_stand_frac
+from src.envs.quadruped_env import QuadrupedEnv
+
+print("Initializing Spot Micro simulation...")
+
+env = QuadrupedEnv()
+
+joint_directions = np.array([-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1])
+
+kinematics = Kinematics()
+
+standby = kinematics.get_default_feet_pos()
+
+body_state = BodyStateT(
+    omega=0,
+    phi=0,
+    psi=0,
+    xm=0,
+    ym=KinConfig.default_body_height,
+    zm=0,
+    px=0,
+    py=0,
+    pz=0,
+    feet=standby,
+    default_feet=standby,
+)
+
+dt = 1.0 / 240
+while True:
+    env.gui.update_gait_state(gait_state)
+    env.gui.update()
+
+    joints = kinematics.inverse_kinematics(body_state)
+    joints *= joint_directions
+
+    _, _, done, truncated, _ = env.step(joints)
+    if done or truncated:
+        env.reset()
+
+    time.sleep(dt)