From f3f3864b83bc8a7e355928db99e7de0d8cc6d8a2 Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Fri, 10 Oct 2025 19:33:01 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=94=A5=20Remove=20simple=20play=20kinemat?= =?UTF-8?q?ics?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- simulation/play.py | 4 +- simulation/play_kinematics.py | 45 ----------------- simulation/simulation/gui.py | 73 ---------------------------- simulation/src/envs/quadruped_env.py | 5 +- simulation/src/utils/gui.py | 8 +-- 5 files changed, 7 insertions(+), 128 deletions(-) delete mode 100644 simulation/play_kinematics.py delete mode 100644 simulation/simulation/gui.py diff --git a/simulation/play.py b/simulation/play.py index a650257..d73ba3a 100644 --- a/simulation/play.py +++ b/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]}") diff --git a/simulation/play_kinematics.py b/simulation/play_kinematics.py deleted file mode 100644 index fcd0dc5..0000000 --- a/simulation/play_kinematics.py +++ /dev/null @@ -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) diff --git a/simulation/simulation/gui.py b/simulation/simulation/gui.py deleted file mode 100644 index c1fea76..0000000 --- a/simulation/simulation/gui.py +++ /dev/null @@ -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, - ) diff --git a/simulation/src/envs/quadruped_env.py b/simulation/src/envs/quadruped_env.py index 67c29c6..bfc81c9 100644 --- a/simulation/src/envs/quadruped_env.py +++ b/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) diff --git a/simulation/src/utils/gui.py b/simulation/src/utils/gui.py index f99bcc1..d2cfeb0 100644 --- a/simulation/src/utils/gui.py +++ b/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()