🐛 Fixes many smaller simulation pains

simulation/play.py

@@ -1,23 +1,66 @@
 import time
 import numpy as np
+import pybullet as p
 
-from src.robot.kinematics import Kinematics, BodyStateT
+from src.robot.kinematics import Kinematics, BodyStateT, KinConfig
-from src.robot.gait import GaitController, GaitStateT
+from src.robot.gait import GaitController, GaitStateT, GaitType, default_offset, default_stand_frac
 from src.envs.quadruped_env import QuadrupedEnv
 
-env = QuadrupedEnv()
+print("Initializing Spot Micro simulation...")
+try:
+    env = QuadrupedEnv()
+    print("Environment created successfully")
+    print(f"Robot ID: {env.robot.robot_id}")
+    print(f"Number of joints: {env.robot.get_observation().shape[0]}")
 
-leg_order = [3, 0, 4, 1, 5, 2]
+    # Print joint names
+    print("\nJoint names:")
+    num_joints = p.getNumJoints(env.robot.robot_id)
+    for i in range(num_joints):
+        joint_info = p.getJointInfo(env.robot.robot_id, i)
+        joint_name = joint_info[1].decode("utf-8")
+        joint_type = joint_info[2]
+        print(f"Joint {i}: {joint_name} (type: {joint_type})")
 
-kinematics = Kinematics(0.605, 0.01, 1.112, 1.185, 2.075, 0.78)
+    print("Simulation ready! Use the GUI sliders to control the robot.")
+except Exception as e:
+    print(f"Error creating environment: {e}")
+    import traceback
+
+    traceback.print_exc()
+    exit(1)
+
+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=0,
+body_state = BodyStateT(
-                        zm=0, px=0, py=0, pz=0, feet=standby, default_feet=standby)
+    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,
+)
 
-gait_state = GaitStateT(step_height=30, step_x=0, step_z=0,
+gait_state = GaitStateT(
-                        step_angle=0, step_velocity=1, step_depth=0.002)
+    step_height=KinConfig.default_step_height,
+    step_x=0,
+    step_z=0,
+    step_angle=0,
+    step_velocity=1,
+    step_depth=KinConfig.default_step_depth,
+    stand_frac=default_stand_frac[GaitType.TROT_GATE],
+    offset=default_offset[GaitType.TROT_GATE],
+    gait_type=GaitType.TROT_GATE,
+)
 
 gait = GaitController(standby)
 
@@ -28,9 +71,8 @@ while True:
     env.gui.update()
 
     gait.step(gait_state, body_state, dt)
-    angles = kinematics.inverse_kinematics(body_state)
+    joints = kinematics.inverse_kinematics(body_state)
-    print(angles)
+    joints = joints * joint_directions
-    joints = angles  # angles.reshape(4, 3)[leg_order].flatten()
 
     _, _, done, truncated, _ = env.step(joints)
     # if done or truncated:

simulation/requirements.txt

@@ -4,3 +4,7 @@ onnxruntime
 tensorflow
 numpy
 matplotlib
+pybullet
+websockets
+msgpack
+asyncio

simulation/src/envs/quadruped_env.py

@@ -2,6 +2,7 @@ import gymnasium as gym
 import pybullet as p
 import pybullet_data
 import numpy as np
+import os
 from enum import Enum
 
 from src.utils.gui import GUI
@@ -16,16 +17,33 @@ class TerrainType(Enum):
 
 class QuadrupedRobot:
     def __init__(self, urdf_path, position=[0, 0, 0.3], orientation=[0, 0, 0], use_fixed_base=False):
+        print(f"Loading URDF from: {urdf_path}")
+        print(f"URDF file exists: {os.path.exists(urdf_path)}")
+
         q_orientation = p.getQuaternionFromEuler(orientation)
-        self.robot_id = p.loadURDF(
+        try:
-            urdf_path, position, q_orientation, useFixedBase=use_fixed_base)
+            self.robot_id = p.loadURDF(urdf_path, position, q_orientation, useFixedBase=use_fixed_base)
+            print(f"Robot loaded successfully with ID: {self.robot_id}")
+            if self.robot_id >= 0:
+                print(f"Number of joints: {p.getNumJoints(self.robot_id)}")
+            else:
+                print("ERROR: Robot ID is negative, URDF loading failed!")
+        except Exception as e:
+            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):
+        return [p.getJointInfo(self.robot_id, idx)[1].decode("utf-8") for idx in self.movable_joint_indices]
 
     def get_observation(self):
         position, orientation = p.getBasePositionAndOrientation(self.robot_id)
         orientation = p.getEulerFromQuaternion(orientation)
         velocity, angular_velocity = p.getBaseVelocity(self.robot_id)
-        joint_states = p.getJointStates(
+        joint_states = p.getJointStates(self.robot_id, range(p.getNumJoints(self.robot_id)))
-            self.robot_id, range(p.getNumJoints(self.robot_id)))
         joint_positions = [state[0] for state in joint_states]
         joint_velocities = [state[1] for state in joint_states]
         return np.concatenate(
@@ -40,10 +58,13 @@ 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]
                 p.setJointMotorControl2(
                     bodyIndex=self.robot_id,
-                jointIndex=i,
+                    jointIndex=joint_index,
                     controlMode=p.POSITION_CONTROL,
                     targetPosition=position,
                     force=50,  # 343 #  / 100 for newtons - Fix mass
@@ -60,8 +81,7 @@ class QuadrupedEnv(gym.Env):
         else:
             p.connect(p.DIRECT)
 
-        self.observation_space = gym.spaces.Box(
+        self.observation_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(48,))
-            low=-np.inf, high=np.inf, shape=(48,))
         self.action_space = gym.spaces.Box(low=-1, high=1, shape=(18,))
         p.setAdditionalSearchPath(pybullet_data.getDataPath())
 
@@ -95,8 +115,7 @@ class QuadrupedEnv(gym.Env):
             p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
             p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION, 1)
             p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
-            self.terrain = p.loadURDF(
+            self.terrain = p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
-                "plane_transparent.urdf", useMaximalCoordinates=True)
         elif terrain_type == TerrainType.TERRAIN:
             terrainShape = p.createCollisionShape(
                 shapeType=p.GEOM_HEIGHTFIELD, meshScale=[0.1, 0.1, 24], fileName="heightmaps/wm_height_out.png"

simulation/src/robot/gait.py

@@ -3,7 +3,10 @@ import numpy as np
 from typing import TypedDict
 from enum import Enum
 
-from src.robot.kinematics import BodyStateT
+try:
+    from src.robot.kinematics import BodyStateT
+except ImportError:
+    from robot.kinematics import BodyStateT
 
 
 class GaitType(Enum):
@@ -34,15 +37,12 @@ class GaitStateT(TypedDict):
     gait_type: GaitType
 
 
-length_multipliers = np.array(
+length_multipliers = np.array([-1.4, -1.0, -1.5, -1.5, -1.5, 0.0, 0.0, 0.0, 1.5, 1.5, 1.4, 1.0])
-    [-1.4, -1.0, -1.5, -1.5, -1.5, 0.0, 0.0, 0.0, 1.5, 1.5, 1.4, 1.0])
+height_profile = np.array([0.0, 0.0, 0.9, 0.9, 0.9, 0.9, 0.9, 1.1, 1.1, 1.1, 0.0, 0.0])
-height_profile = np.array(
-    [0.0, 0.0, 0.9, 0.9, 0.9, 0.9, 0.9, 1.1, 1.1, 1.1, 0.0, 0.0])
 
 
 def sine_curve(length, angle, height, phase):
-    x, z = length * (1 - 2 * phase) * np.cos(angle), length * \
+    x, z = length * (1 - 2 * phase) * np.cos(angle), length * (1 - 2 * phase) * np.sin(angle)
-        (1 - 2 * phase) * np.sin(angle)
     y = height * np.cos(np.pi * (x + z) / (2 * length)) if length else 0
     return np.array([x, z, y])
 
@@ -67,8 +67,7 @@ def get_control_points(length, angle, height):
 def bezier_curve(length, angle, height, phase):
     ctrl = get_control_points(length, angle, height)
     n = len(ctrl) - 1
-    coeffs = np.array([math.comb(n, i) * (phase**i) *
+    coeffs = np.array([math.comb(n, i) * (phase**i) * ((1 - phase) ** (n - i)) for i in range(n + 1)])
-                      ((1 - phase) ** (n - i)) for i in range(n + 1)])
     return np.sum(ctrl * coeffs[:, None], axis=0)
 
 
@@ -80,8 +79,7 @@ class GaitController:
     def step(self, gait: GaitStateT, body: BodyStateT, dt: float):
         step_x, step_z, angle = gait["step_x"], gait["step_z"], gait["step_angle"]
         if not any((step_x, step_z, angle)):
-            body["feet"] = body["feet"] + \
+            body["feet"] = body["feet"] + (self.default_position - body["feet"]) * dt * 10
-                (self.default_position - body["feet"]) * dt * 10
             self.phase = 0.0
             return
 
@@ -101,12 +99,10 @@ class GaitController:
 
         for i, (default_foot, current_foot) in enumerate(zip(self.default_position, body["feet"])):
             phase = (self.phase + offsets[i]) % 1
-            ph_norm, curve_fn, amp = self._phase_params(
+            ph_norm, curve_fn, amp = self._phase_params(phase, stand_fraction, depth, height)
-                phase, stand_fraction, depth, height)
             delta_pos = curve_fn(length / 2, turn_amplitude, amp, ph_norm)
-            delta_rot = curve_fn(np.rad2deg(angle), yaw_arc(
+            delta_rot = curve_fn(np.rad2deg(angle), yaw_arc(default_foot, current_foot), amp, ph_norm)
-                default_foot, current_foot), amp, ph_norm)
+            new_feet[i][:2] = default_foot[:2] + delta_pos[:2] + delta_rot[:2]
-            new_feet[i][:2] = default_foot[:2] + delta_pos + delta_rot
             # new_feet[i][3] = 1
 
         body["feet"] = new_feet

simulation/src/robot/kinematics.py

@@ -1,7 +1,41 @@
 import numpy as np
 from typing import TypedDict, List
 
-import config
+
+class KinConfig:
+    # SPOTMICRO_ESP32 configuration (matching C++ version)
+    coxa = 60.5 / 100.0
+    coxa_offset = 10.0 / 100.0
+    femur = 111.2 / 100.0
+    tibia = 118.5 / 100.0
+    L = 207.5 / 100.0
+    W = 78.0 / 100.0
+
+    mountOffsets = [[L / 2, 0, W / 2], [L / 2, 0, -W / 2], [-L / 2, 0, W / 2], [-L / 2, 0, -W / 2]]
+
+    default_feet_positions = [
+        [mountOffsets[0][0], 0, mountOffsets[0][2] + coxa, 1],
+        [mountOffsets[1][0], 0, mountOffsets[1][2] - coxa, 1],
+        [mountOffsets[2][0], 0, mountOffsets[2][2] + coxa, 1],
+        [mountOffsets[3][0], 0, mountOffsets[3][2] - coxa, 1],
+    ]
+
+    # Max constants
+    max_roll = 15 * np.pi / 2
+    max_pitch = 15 * np.pi / 2
+    max_body_shift_x = W / 3
+    max_body_shift_z = W / 3
+    max_leg_reach = femur + tibia - coxa_offset
+    min_body_height = max_leg_reach * 0.45
+    max_body_height = max_leg_reach * 0.9
+    body_height_range = max_body_height - min_body_height
+    max_step_length = max_leg_reach * 0.8
+    max_step_height = max_leg_reach / 2
+
+    # Default constants
+    default_step_depth = 0.002
+    default_body_height = min_body_height + body_height_range / 2
+    default_step_height = default_body_height / 2
 
 
 class BodyStateT(TypedDict):
@@ -56,68 +90,69 @@ def get_transformation_matrix(body_state):
 
 
 class Kinematics:
-    def __init__(self, l1, l2, l3, l4, length, width):
+    def __init__(self):
-        self.l1 = float(l1)
+        # Use KinConfig constants (matching C++ version)
-        self.l2 = float(l2)
+        self.coxa = KinConfig.coxa
-        self.l3 = float(l3)
+        self.coxa_offset = KinConfig.coxa_offset
-        self.l4 = float(l4)
+        self.femur = KinConfig.femur
-        self.length = float(length)
+        self.tibia = KinConfig.tibia
-        self.width = float(width)
+        self.L = KinConfig.L
-        self.deg2rad = np.pi / 180
+        self.W = KinConfig.W
 
-        self.mount_offsets = np.array([
+        self.mount_offsets = np.array(KinConfig.mountOffsets)
-            [self.length / 2, 0,  self.width / 2],
-            [self.length / 2, 0, -self.width / 2],
-            [-self.length / 2, 0,  self.width / 2],
-            [-self.length / 2, 0, -self.width / 2]
-        ])
 
-        self.inv_mount_rot = np.array([
+        self.inv_mount_rot = np.array([[0, 0, -1], [0, 1, 0], [1, 0, 0]])
-            [0, 0, -1],
+
-            [0, 1,  0],
+        # Cache for current state (matching C++ optimization)
-            [1, 0,  0]
+        self.current_state = None
-        ])
 
     def get_default_feet_pos(self):
-        feet = self.mount_offsets.copy()
+        # Return default feet positions matching C++ version exactly
-        feet[:, 1] = -1
+        feet = []
-        feet[:, 2] += np.array([self.l1, -self.l1,  self.l1, -self.l1])
+        for i, offset in enumerate(self.mount_offsets):
-        return feet
+            y_pos = 0  # Ground level (matching C++ default_feet_positions)
+            z_offset = -self.coxa if i % 2 == 1 else self.coxa
+            feet.append([offset[0], y_pos, offset[2] + z_offset])
+        return np.array(feet)
 
     def inverse_kinematics(self, body_state):
-        roll, pitch, yaw = np.deg2rad(body_state["omega"]), np.deg2rad(
+        # Check if state has changed (optimization from C++ version)
-            body_state["phi"]), np.deg2rad(body_state["psi"])
+        # if self.current_state == body_state:
+        #     return self.current_angles if hasattr(self, "current_angles") else []
+
+        self.current_state = body_state.copy()
+
+        roll, pitch, yaw = np.deg2rad(body_state["omega"]), np.deg2rad(body_state["phi"]), np.deg2rad(body_state["psi"])
         xm, ym, zm = body_state["xm"], body_state["ym"], body_state["zm"]
 
         rot = self._rotation_matrix(roll, pitch, yaw)
         inv_rot = rot.T
-        inv_tr = - \
+        inv_tr = -inv_rot @ np.array([xm, ym, zm])
-            inv_rot @ np.array([xm, ym, zm])
 
         angles = []
         for idx, foot_world in enumerate(body_state["feet"]):
             foot_body = inv_rot @ foot_world + inv_tr
-            foot_local = self.inv_mount_rot @ (foot_body -
+            foot_local = self.inv_mount_rot @ (foot_body - self.mount_offsets[idx])
-                                               self.mount_offsets[idx])
             x_local = -foot_local[0] if idx % 2 else foot_local[0]
-            angles.extend(self._leg_ik(x_local, foot_local[1], foot_local[2]))
+            leg_angles = self._leg_ik(x_local, foot_local[1], foot_local[2])
+            angles.extend(leg_angles)
+
+        self.current_angles = angles
         return angles
 
     def _leg_ik(self, x, y, z):
-        f = np.sqrt(max(0.0, x*x + y*y - self.l1*self.l1))
+        # Match C++ implementation exactly
-        g = f - self.l2
+        F = np.sqrt(max(0.0, x * x + y * y - self.coxa * self.coxa))
-        h = np.sqrt(g*g + z*z)
+        G = F - self.coxa_offset
+        H = np.sqrt(G * G + z * z)
 
-        t1 = -np.arctan2(y, x) - np.arctan2(f, -self.l1)
+        theta1 = -np.arctan2(y, x) - np.arctan2(F, -self.coxa)
+        D = (H * H - self.femur * self.femur - self.tibia * self.tibia) / (2 * self.femur * self.tibia)
+        theta3 = np.arccos(max(-1.0, min(1.0, D)))
+        theta2 = np.arctan2(z, G) - np.arctan2(self.tibia * np.sin(theta3), self.femur + self.tibia * np.cos(theta3))
 
-        d = (h*h - self.l3*self.l3 - self.l4*self.l4) / (2*self.l3*self.l4)
+        # Return angles in radians (matching web app)
-        d = max(-1.0, min(1.0, d))
+        return theta1, theta2, theta3
-
-        t3 = np.arccos(d)
-        t2 = np.arctan2(z, g) - np.arctan2(self.l4*np.sin(t3),
-                                           self.l3 + self.l4*np.cos(t3))
-
-        return t1, t2, t3
 
     def _rotation_matrix(self, roll, pitch, yaw):
         cr, sr = np.cos(roll), np.sin(roll)

simulation/src/utils/gui.py

@@ -1,7 +1,7 @@
 import pybullet as p
 import numpy as np
 
-from src.robot.kinematics import BodyStateT
+from src.robot.kinematics import BodyStateT, KinConfig
 from src.robot.gait import GaitStateT, GaitType, default_stand_frac, default_offset
 
 
@@ -10,36 +10,28 @@ class GUI:
         self.robot = bot
         self.c_yaw = 10
         self.c_pitch = -17
-        self.c_distance = 5
+        self.c_distance = 1
 
-        self.x_slider = p.addUserDebugParameter("x", -50, 50, 0)
+        self.x_slider = p.addUserDebugParameter("x", -1, 1, 0)
-        self.y_slider = p.addUserDebugParameter("y", -50, 50, 0)
+        self.y_slider = p.addUserDebugParameter("y", 0, 1, 0.5)
-        self.z_slider = p.addUserDebugParameter("z", -50, 50, 0)
+        self.z_slider = p.addUserDebugParameter("z", -1, 1, 0)
-        self.yaw_slider = p.addUserDebugParameter(
+        self.yaw_slider = p.addUserDebugParameter("yaw", -1, 1, 0)
-            "yaw", -np.pi / 4, np.pi / 4, 0)
+        self.pitch_slider = p.addUserDebugParameter("pitch", -1, 1, 0)
-        self.pitch_slider = p.addUserDebugParameter(
+        self.roll_slider = p.addUserDebugParameter("roll", -1, 1, 0)
-            "pitch", -np.pi / 4, np.pi / 4, 0)
-        self.roll_slider = p.addUserDebugParameter(
-            "roll", -np.pi / 4, np.pi / 4, 0)
 
-        self.pivot_x_slider = p.addUserDebugParameter("pivot x", -50, 50, 0)
+        self.pivot_x_slider = p.addUserDebugParameter("pivot x", -1, 1, 0)
-        self.pivot_y_slider = p.addUserDebugParameter("pivot y", -50, 50, 0)
+        self.pivot_y_slider = p.addUserDebugParameter("pivot y", -1, 1, 0)
-        self.pivot_z_slider = p.addUserDebugParameter("pivot z", -50, 50, 0)
+        self.pivot_z_slider = p.addUserDebugParameter("pivot z", -1, 1, 0)
 
-        self.step_x_slider = p.addUserDebugParameter("Step x", -50, 50, 0)
+        self.step_x_slider = p.addUserDebugParameter("Step x", -1, 1, 0)
-        self.step_z_slider = p.addUserDebugParameter("Step z", -50, 50, 0)
+        self.step_z_slider = p.addUserDebugParameter("Step z", -1, 1, 0)
-        self.angle_slider = p.addUserDebugParameter(
+        self.angle_slider = p.addUserDebugParameter("Angle", -1, 1, 0)
-            "Angle", -np.pi / 4, np.pi / 4, 0)
+        self.step_height_slider = p.addUserDebugParameter("Step height", 0, 50, 15)
-        self.step_height_slider = p.addUserDebugParameter(
+        self.step_depth_slider = p.addUserDebugParameter("Step depth", 0, 0.01, 0.002)
-            "Step height", 0, 50, 15)
-        self.step_depth_slider = p.addUserDebugParameter(
-            "Step depth", 0, 0.01, 0.002)
         self.speed_slider = p.addUserDebugParameter("Speed", 0, 2, 1)
-        self.stand_frac_slider = p.addUserDebugParameter(
+        self.stand_frac_slider = p.addUserDebugParameter("Stand frac", 0, 1, 0.5)
-            "Stand frac", 0, 1, 0.5)
 
-        self.gait_type_slider = p.addUserDebugParameter(
+        self.gait_type_slider = p.addUserDebugParameter("Gait Type", 0, len(GaitType) - 1, 0)
-            "Gait Type", 0, len(GaitType) - 1, 0)
 
         # self.gait_type_slider = p.addUserDebugParameter("Gait Type", 0, len(GaitType) - 1, 0, paramType=p.GUI_ENUM,
         #                               enumNames=[g.value for g in GaitType])
@@ -49,35 +41,31 @@ class GUI:
         gait_state["step_x"] = p.readUserDebugParameter(self.step_x_slider)
         gait_state["step_z"] = p.readUserDebugParameter(self.step_z_slider)
         gait_state["step_angle"] = p.readUserDebugParameter(self.angle_slider)
-        gait_state["step_height"] = p.readUserDebugParameter(
+        gait_state["step_height"] = p.readUserDebugParameter(self.step_height_slider)
-            self.step_height_slider)
+        gait_state["step_depth"] = p.readUserDebugParameter(self.step_depth_slider)
-        gait_state["step_depth"] = p.readUserDebugParameter(
+        gait_state["step_velocity"] = p.readUserDebugParameter(self.speed_slider)
-            self.step_depth_slider)
+        gait_state["stand_frac"] = p.readUserDebugParameter(self.stand_frac_slider)
-        gait_state["step_velocity"] = p.readUserDebugParameter(
-            self.speed_slider)
-        gait_state["stand_frac"] = p.readUserDebugParameter(
-            self.stand_frac_slider)
         gait_state["offset"] = default_offset[self.last_gait_type]
 
     def update_body_state(self, body_state: BodyStateT):
-        body_state["xm"] = p.readUserDebugParameter(self.x_slider)
+        body_state["xm"] = p.readUserDebugParameter(self.x_slider) * KinConfig.max_body_shift_x
-        body_state["ym"] = p.readUserDebugParameter(self.y_slider)
+        body_state["ym"] = (
-        body_state["zm"] = p.readUserDebugParameter(self.z_slider)
+            p.readUserDebugParameter(self.y_slider) * KinConfig.body_height_range + KinConfig.min_body_height
-        body_state["omega"] = p.readUserDebugParameter(self.roll_slider)
+        )
-        body_state["phi"] = p.readUserDebugParameter(self.pitch_slider)
+        body_state["zm"] = p.readUserDebugParameter(self.z_slider) * KinConfig.max_body_shift_z
-        body_state["psi"] = p.readUserDebugParameter(self.yaw_slider)
+        body_state["omega"] = p.readUserDebugParameter(self.roll_slider) * KinConfig.max_roll
-        body_state["px"] = p.readUserDebugParameter(self.pivot_x_slider)
+        body_state["phi"] = p.readUserDebugParameter(self.pitch_slider) * KinConfig.max_pitch
-        body_state["py"] = p.readUserDebugParameter(self.pivot_y_slider)
+        body_state["psi"] = p.readUserDebugParameter(self.yaw_slider) * KinConfig.max_pitch
-        body_state["pz"] = p.readUserDebugParameter(self.pivot_z_slider)
+        body_state["px"] = p.readUserDebugParameter(self.pivot_x_slider) * KinConfig.max_body_shift_x
+        body_state["py"] = p.readUserDebugParameter(self.pivot_y_slider) * KinConfig.max_body_shift_x
+        body_state["pz"] = p.readUserDebugParameter(self.pivot_z_slider) * KinConfig.max_body_shift_z
 
     def update(self):
-        gait_type = GaitType(
+        gait_type = GaitType(int(p.readUserDebugParameter(self.gait_type_slider)))
-            int(p.readUserDebugParameter(self.gait_type_slider)))
         if gait_type != self.last_gait_type:
             self.last_gait_type = gait_type
             p.removeUserDebugItem(self.stand_frac_slider)
-            self.stand_frac_slider = p.addUserDebugParameter(
+            self.stand_frac_slider = p.addUserDebugParameter("Stand frac", 0, 1, default_stand_frac[gait_type])
-                "Stand frac", 0, 1, default_stand_frac[gait_type])
 
         quadruped_pos, _ = p.getBasePositionAndOrientation(self.robot)
         p.resetDebugVisualizerCamera(

simulation/test.py

@@ -1,4 +1,4 @@
-from src.envs.environment import QuadrupedEnv
+from src.envs.quadruped_env import QuadrupedEnv
 from training.model import SimpleNN
 
 import resources as resources