From 51ee910fb65f35e1092324350bca150bff386274 Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Fri, 10 Oct 2025 18:19:14 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Fixes=20many=20smaller=20simulat?= =?UTF-8?q?ion=20pains?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- simulation/play.py | 68 ++++++++++++--- simulation/requirements.txt | 6 +- simulation/src/__init__.py | 0 simulation/src/envs/quadruped_env.py | 53 ++++++++---- simulation/src/robot/gait.py | 28 +++---- simulation/src/robot/kinematics.py | 121 +++++++++++++++++---------- simulation/src/utils/gui.py | 82 ++++++++---------- simulation/test.py | 2 +- 8 files changed, 222 insertions(+), 138 deletions(-) create mode 100644 simulation/src/__init__.py diff --git a/simulation/play.py b/simulation/play.py index 6800ed6..a181833 100644 --- a/simulation/play.py +++ b/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) diff --git a/simulation/requirements.txt b/simulation/requirements.txt index 8019ca5..436d9c2 100644 --- a/simulation/requirements.txt +++ b/simulation/requirements.txt @@ -3,4 +3,8 @@ onnx onnxruntime tensorflow numpy -matplotlib \ No newline at end of file +matplotlib +pybullet +websockets +msgpack +asyncio \ No newline at end of file diff --git a/simulation/src/__init__.py b/simulation/src/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/simulation/src/envs/quadruped_env.py b/simulation/src/envs/quadruped_env.py index 01fa1f5..67c29c6 100644 --- a/simulation/src/envs/quadruped_env.py +++ b/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( - urdf_path, position, q_orientation, useFixedBase=use_fixed_base) + try: + 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( - self.robot_id, range(p.getNumJoints(self.robot_id))) + joint_states = p.getJointStates(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,16 +58,19 @@ class QuadrupedRobot: ) def apply_action(self, action): + # Apply target positions (radians) to actuated joints in fixed order for i, position in enumerate(action): - p.setJointMotorControl2( - bodyIndex=self.robot_id, - jointIndex=i, - controlMode=p.POSITION_CONTROL, - targetPosition=position, - force=50, # 343 # / 100 for newtons - Fix mass - positionGain=0.5, - maxVelocity=13.09, - ) + if i < len(self.movable_joint_indices): + joint_index = self.movable_joint_indices[i] + p.setJointMotorControl2( + bodyIndex=self.robot_id, + jointIndex=joint_index, + controlMode=p.POSITION_CONTROL, + targetPosition=position, + force=50, # 343 # / 100 for newtons - Fix mass + positionGain=0.5, + maxVelocity=13.09, + ) class QuadrupedEnv(gym.Env): @@ -60,8 +81,7 @@ class QuadrupedEnv(gym.Env): else: p.connect(p.DIRECT) - self.observation_space = gym.spaces.Box( - low=-np.inf, high=np.inf, shape=(48,)) + self.observation_space = gym.spaces.Box(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( - "plane_transparent.urdf", useMaximalCoordinates=True) + self.terrain = p.loadURDF("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" diff --git a/simulation/src/robot/gait.py b/simulation/src/robot/gait.py index 7599920..0e5d6b0 100644 --- a/simulation/src/robot/gait.py +++ b/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( - [-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]) +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]) +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 * \ - (1 - 2 * phase) * np.sin(angle) + x, z = length * (1 - 2 * phase) * np.cos(angle), length * (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) * - ((1 - phase) ** (n - i)) for i in range(n + 1)]) + coeffs = np.array([math.comb(n, i) * (phase**i) * ((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"] + \ - (self.default_position - body["feet"]) * dt * 10 + body["feet"] = body["feet"] + (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( - phase, stand_fraction, depth, height) + ph_norm, curve_fn, amp = self._phase_params(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( - default_foot, current_foot), amp, ph_norm) - new_feet[i][:2] = default_foot[:2] + delta_pos + delta_rot + delta_rot = curve_fn(np.rad2deg(angle), yaw_arc(default_foot, current_foot), amp, ph_norm) + new_feet[i][:2] = default_foot[:2] + delta_pos[:2] + delta_rot[:2] # new_feet[i][3] = 1 body["feet"] = new_feet diff --git a/simulation/src/robot/kinematics.py b/simulation/src/robot/kinematics.py index ef79e9b..3e7a18c 100644 --- a/simulation/src/robot/kinematics.py +++ b/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): - self.l1 = float(l1) - self.l2 = float(l2) - self.l3 = float(l3) - self.l4 = float(l4) - self.length = float(length) - self.width = float(width) - self.deg2rad = np.pi / 180 + def __init__(self): + # Use KinConfig constants (matching C++ version) + self.coxa = KinConfig.coxa + self.coxa_offset = KinConfig.coxa_offset + self.femur = KinConfig.femur + self.tibia = KinConfig.tibia + self.L = KinConfig.L + self.W = KinConfig.W - self.mount_offsets = np.array([ - [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.mount_offsets = np.array(KinConfig.mountOffsets) - self.inv_mount_rot = np.array([ - [0, 0, -1], - [0, 1, 0], - [1, 0, 0] - ]) + self.inv_mount_rot = np.array([[0, 0, -1], [0, 1, 0], [1, 0, 0]]) + + # Cache for current state (matching C++ optimization) + self.current_state = None def get_default_feet_pos(self): - feet = self.mount_offsets.copy() - feet[:, 1] = -1 - feet[:, 2] += np.array([self.l1, -self.l1, self.l1, -self.l1]) - return feet + # Return default feet positions matching C++ version exactly + feet = [] + for i, offset in enumerate(self.mount_offsets): + 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( - body_state["phi"]), np.deg2rad(body_state["psi"]) + # Check if state has changed (optimization from C++ version) + # 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_rot @ np.array([xm, ym, zm]) + inv_tr = -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 - - self.mount_offsets[idx]) + foot_local = self.inv_mount_rot @ (foot_body - 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)) - g = f - self.l2 - h = np.sqrt(g*g + z*z) + # Match C++ implementation exactly + F = np.sqrt(max(0.0, x * x + y * y - self.coxa * self.coxa)) + 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) - d = max(-1.0, min(1.0, d)) - - 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 + # Return angles in radians (matching web app) + return theta1, theta2, theta3 def _rotation_matrix(self, roll, pitch, yaw): cr, sr = np.cos(roll), np.sin(roll) diff --git a/simulation/src/utils/gui.py b/simulation/src/utils/gui.py index d753a9c..b3ab811 100644 --- a/simulation/src/utils/gui.py +++ b/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.y_slider = p.addUserDebugParameter("y", -50, 50, 0) - self.z_slider = p.addUserDebugParameter("z", -50, 50, 0) - self.yaw_slider = p.addUserDebugParameter( - "yaw", -np.pi / 4, np.pi / 4, 0) - self.pitch_slider = p.addUserDebugParameter( - "pitch", -np.pi / 4, np.pi / 4, 0) - self.roll_slider = p.addUserDebugParameter( - "roll", -np.pi / 4, np.pi / 4, 0) + self.x_slider = p.addUserDebugParameter("x", -1, 1, 0) + self.y_slider = p.addUserDebugParameter("y", 0, 1, 0.5) + self.z_slider = p.addUserDebugParameter("z", -1, 1, 0) + self.yaw_slider = p.addUserDebugParameter("yaw", -1, 1, 0) + self.pitch_slider = p.addUserDebugParameter("pitch", -1, 1, 0) + self.roll_slider = p.addUserDebugParameter("roll", -1, 1, 0) - self.pivot_x_slider = p.addUserDebugParameter("pivot x", -50, 50, 0) - self.pivot_y_slider = p.addUserDebugParameter("pivot y", -50, 50, 0) - self.pivot_z_slider = p.addUserDebugParameter("pivot z", -50, 50, 0) + self.pivot_x_slider = p.addUserDebugParameter("pivot x", -1, 1, 0) + self.pivot_y_slider = p.addUserDebugParameter("pivot y", -1, 1, 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_z_slider = p.addUserDebugParameter("Step z", -50, 50, 0) - self.angle_slider = p.addUserDebugParameter( - "Angle", -np.pi / 4, np.pi / 4, 0) - self.step_height_slider = p.addUserDebugParameter( - "Step height", 0, 50, 15) - self.step_depth_slider = p.addUserDebugParameter( - "Step depth", 0, 0.01, 0.002) + self.step_x_slider = p.addUserDebugParameter("Step x", -1, 1, 0) + self.step_z_slider = p.addUserDebugParameter("Step z", -1, 1, 0) + self.angle_slider = p.addUserDebugParameter("Angle", -1, 1, 0) + self.step_height_slider = p.addUserDebugParameter("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( - "Stand frac", 0, 1, 0.5) + self.stand_frac_slider = p.addUserDebugParameter("Stand frac", 0, 1, 0.5) - self.gait_type_slider = p.addUserDebugParameter( - "Gait Type", 0, len(GaitType) - 1, 0) + self.gait_type_slider = p.addUserDebugParameter("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( - self.step_height_slider) - gait_state["step_depth"] = p.readUserDebugParameter( - self.step_depth_slider) - gait_state["step_velocity"] = p.readUserDebugParameter( - self.speed_slider) - gait_state["stand_frac"] = p.readUserDebugParameter( - self.stand_frac_slider) + gait_state["step_height"] = p.readUserDebugParameter(self.step_height_slider) + gait_state["step_depth"] = p.readUserDebugParameter(self.step_depth_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["ym"] = p.readUserDebugParameter(self.y_slider) - body_state["zm"] = p.readUserDebugParameter(self.z_slider) - body_state["omega"] = p.readUserDebugParameter(self.roll_slider) - body_state["phi"] = p.readUserDebugParameter(self.pitch_slider) - body_state["psi"] = p.readUserDebugParameter(self.yaw_slider) - body_state["px"] = p.readUserDebugParameter(self.pivot_x_slider) - body_state["py"] = p.readUserDebugParameter(self.pivot_y_slider) - body_state["pz"] = p.readUserDebugParameter(self.pivot_z_slider) + body_state["xm"] = p.readUserDebugParameter(self.x_slider) * KinConfig.max_body_shift_x + body_state["ym"] = ( + p.readUserDebugParameter(self.y_slider) * KinConfig.body_height_range + KinConfig.min_body_height + ) + body_state["zm"] = p.readUserDebugParameter(self.z_slider) * KinConfig.max_body_shift_z + body_state["omega"] = p.readUserDebugParameter(self.roll_slider) * KinConfig.max_roll + body_state["phi"] = p.readUserDebugParameter(self.pitch_slider) * KinConfig.max_pitch + body_state["psi"] = p.readUserDebugParameter(self.yaw_slider) * KinConfig.max_pitch + 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( - int(p.readUserDebugParameter(self.gait_type_slider))) + gait_type = GaitType(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( - "Stand frac", 0, 1, default_stand_frac[gait_type]) + self.stand_frac_slider = p.addUserDebugParameter("Stand frac", 0, 1, default_stand_frac[gait_type]) quadruped_pos, _ = p.getBasePositionAndOrientation(self.robot) p.resetDebugVisualizerCamera( diff --git a/simulation/test.py b/simulation/test.py index 89dfe25..19c4715 100644 --- a/simulation/test.py +++ b/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