🐛 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.gait import GaitController, GaitStateT
+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
 
-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,
-                        zm=0, px=0, py=0, pz=0, feet=standby, default_feet=standby)
+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,
+)
 
-gait_state = GaitStateT(step_height=30, step_x=0, step_z=0,
-                        step_angle=0, step_velocity=1, step_depth=0.002)
+gait_state = GaitStateT(
+    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,12 +71,11 @@ while True:
     env.gui.update()
 
     gait.step(gait_state, body_state, dt)
-    angles = kinematics.inverse_kinematics(body_state)
-    print(angles)
-    joints = angles  # angles.reshape(4, 3)[leg_order].flatten()
+    joints = kinematics.inverse_kinematics(body_state)
+    joints = joints * joint_directions
 
     _, _, done, truncated, _ = env.step(joints)
     # if done or truncated:
-    #     env.reset()
+    #    env.reset()
 
     time.sleep(dt)