🔥 Remove simple play kinematics

simulation/play.py

@@ -4,11 +4,11 @@ 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
+from src.envs.quadruped_env import QuadrupedEnv, TerrainType
 
 print("Initializing Spot Micro simulation...")
 try:
-    env = QuadrupedEnv()
+    env = QuadrupedEnv(terrain_type=TerrainType.FLAT)
     print("Environment created successfully")
     print(f"Robot ID: {env.robot.robot_id}")
     print(f"Number of joints: {env.robot.get_observation().shape[0]}")

simulation/play_kinematics.py

@@ -1,45 +0,0 @@
-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)

simulation/simulation/gui.py

@@ -1,73 +0,0 @@
-import pybullet as pb
-import numpy as np
-import sys
-
-
-class GUI:
-    def __init__(self, quadruped):
-        self.cameraYaw = 0
-        self.cameraPitch = -7
-        self.cameraDist = 0.66
-
-        self.rollId = pb.addUserDebugParameter("roll", -np.pi / 4, np.pi / 4, 0.0)
-        self.pitchId = pb.addUserDebugParameter("pitch", -np.pi / 4, np.pi / 4, 0.0)
-        self.yawId = pb.addUserDebugParameter("yaw", -np.pi / 4, np.pi / 4, 0.0)
-        self.xId = pb.addUserDebugParameter("x", -0.10, 0.10, 0.0)
-        self.yId = pb.addUserDebugParameter("y", -0.10, 0.10, 0.0)
-        self.zId = pb.addUserDebugParameter("z", -0.055, 0.17, 0.0)
-
-        self.quadruped = quadruped
-
-    def updateCamera(self):
-        quadruped_position, _ = pb.getBasePositionAndOrientation(self.quadruped)
-        pb.resetDebugVisualizerCamera(
-            cameraDistance=self.cameraDist,
-            cameraYaw=self.cameraYaw,
-            cameraPitch=self.cameraPitch,
-            cameraTargetPosition=quadruped_position,
-        )
-
-    def handleInput(self):
-        keys = pb.getKeyboardEvents()
-        # Keys to change camera
-        if keys.get(100):  # D
-            self.cameraYaw += 1
-        if keys.get(97):  # A
-            self.cameraYaw -= 1
-        if keys.get(99):  # C
-            self.cameraPitch += 1
-        if keys.get(102):  # F
-            self.cameraPitch -= 1
-        if keys.get(122):  # Z
-            self.cameraDist += 0.01
-        if keys.get(120):  # X
-            self.cameraDist -= 0.01
-        if keys.get(27):  # ESC
-            pb.disconnect()
-            sys.exit()
-
-    def update(self):
-        self.updateCamera()
-        self.handleInput()
-
-    def getInput(self):
-        self.update()
-        position = np.array(
-            [
-                pb.readUserDebugParameter(self.xId),
-                pb.readUserDebugParameter(self.yId),
-                pb.readUserDebugParameter(self.zId),
-            ]
-        )
-        orientation = np.array(
-            [
-                pb.readUserDebugParameter(self.rollId),
-                pb.readUserDebugParameter(self.pitchId),
-                pb.readUserDebugParameter(self.yawId),
-            ]
-        )
-
-        return (
-            position,
-            orientation,
-        )

simulation/src/envs/quadruped_env.py

@@ -32,8 +32,6 @@ class QuadrupedRobot:
             print(f"Error loading URDF: {e}")
             raise
 
-        # Indices of the 12 actuated joints in the URDF (PyBullet joint indices)
-        # Order: FL(shoulder, leg, foot), FR(shoulder, leg, foot), RL(shoulder, leg, foot), RR(shoulder, leg, foot)
         self.movable_joint_indices = [2, 3, 5, 7, 8, 10, 12, 13, 15, 17, 18, 20]
 
     def get_movable_joint_names(self):
@@ -58,7 +56,6 @@ class QuadrupedRobot:
         )
 
     def apply_action(self, action):
-        # Apply target positions (radians) to actuated joints in fixed order
         for i, position in enumerate(action):
             if i < len(self.movable_joint_indices):
                 joint_index = self.movable_joint_indices[i]
@@ -125,7 +122,7 @@ class QuadrupedEnv(gym.Env):
             p.changeVisualShape(self.terrain, -1, textureUniqueId=textureId)
         elif terrain_type == TerrainType.MAZE:
             terrainShape = p.createCollisionShape(
-                shapeType=p.GEOM_HEIGHTFIELD, meshScale=[1, 1, 3], fileName="heightmaps/Maze.png"
+                shapeType=p.GEOM_HEIGHTFIELD, meshScale=[0.2, 0.2, 0.5], fileName="heightmaps/Maze.png"
             )
             textureId = p.loadTexture("heightmaps/Maze.png")
             maze = p.createMultiBody(0, terrainShape)

simulation/src/utils/gui.py

@@ -67,13 +67,13 @@ class GUI:
 
         keys = p.getKeyboardEvents()
         if keys.get(ord("j")):
-            self.c_yaw += 0.1
+            self.c_yaw += 0.3
         if keys.get(ord("k")):
-            self.c_yaw -= 0.1
+            self.c_yaw -= 0.3
         if keys.get(ord("m")):
-            self.c_pitch += 0.1
+            self.c_pitch += 0.3
         if keys.get(ord("i")):
-            self.c_pitch -= 0.1
+            self.c_pitch -= 0.3
 
         if keys.get(ord("q")) or keys.get(27):
             p.disconnect()