diff --git a/.gitignore b/.gitignore index ed3d9d1..5a634aa 100644 --- a/.gitignore +++ b/.gitignore @@ -222,3 +222,4 @@ cython_debug/ marimo/_static/ marimo/_lsp/ __marimo__/ +sim/new_pid_sim/MUJOCO_LOG.TXT diff --git a/object_detection/detect_people.py b/object_detection/detect_people.py deleted file mode 100644 index b01c1d9..0000000 --- a/object_detection/detect_people.py +++ /dev/null @@ -1,37 +0,0 @@ -import cv2 -from ultralytics import YOLO - - -model = YOLO("yolo11n.pt") -cap = cv2.VideoCapture(0) - -while True: - ret, frame = cap.read() - if not ret: - break - results = model(frame, verbose=False) - for r in results: - boxes = r.boxes - for box in boxes: - cls = int(box.cls[0]) - label = model.names[cls] - if label != "person": - continue # only show people - - x1, y1, x2, y2 = map(int, box.xyxy[0]) - conf = float(box.conf[0]) - - # Draw bounding box and label - cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 255, 0), 2) - cv2.putText(frame, f"{label} {conf:.2f}", (x1, y1 - 10), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2) - - # Show the frame - cv2.imshow("YOLOv11 People Detection", frame) - - # Press 'q' to quit - if cv2.waitKey(1) & 0xFF == ord('q'): - break - -cap.release() -cv2.destroyAllWindows() \ No newline at end of file diff --git a/object_detection/hand-keypoints.yaml b/object_detection/hand-keypoints.yaml deleted file mode 100644 index 8c7a0ec..0000000 --- a/object_detection/hand-keypoints.yaml +++ /dev/null @@ -1,51 +0,0 @@ -# Ultralytics 🚀 AGPL-3.0 License - https://ultralytics.com/license - -# Hand Keypoints dataset by Ultralytics -# Documentation: https://docs.ultralytics.com/datasets/pose/hand-keypoints/ -# Example usage: yolo train data=hand-keypoints.yaml -# parent -# ├── ultralytics -# └── datasets -# └── hand-keypoints ← downloads here (369 MB) - -# Train/val/test sets as 1) dir: path/to/imgs, 2) file: path/to/imgs.txt, or 3) list: [path/to/imgs1, path/to/imgs2, ..] -path: hand-keypoints # dataset root dir -train: images/train # train images (relative to 'path') 18776 images -val: images/val # val images (relative to 'path') 7992 images - -# Keypoints -kpt_shape: [21, 3] # number of keypoints, number of dims (2 for x,y or 3 for x,y,visible) -flip_idx: - [0, 1, 2, 4, 3, 10, 11, 12, 13, 14, 5, 6, 7, 8, 9, 15, 16, 17, 18, 19, 20] - -# Classes -names: - 0: hand - -# Keypoint names per class -kpt_names: - 0: - - wrist - - thumb_cmc - - thumb_mcp - - thumb_ip - - thumb_tip - - index_mcp - - index_pip - - index_dip - - index_tip - - middle_mcp - - middle_pip - - middle_dip - - middle_tip - - ring_mcp - - ring_pip - - ring_dip - - ring_tip - - pinky_mcp - - pinky_pip - - pinky_dip - - pinky_tip - -# Download script/URL (optional) -download: https://github.com/ultralytics/assets/releases/download/v0.0.0/hand-keypoints.zip diff --git a/object_detection/hand_gestures.py b/object_detection/hand_gestures.py deleted file mode 100644 index d36260b..0000000 --- a/object_detection/hand_gestures.py +++ /dev/null @@ -1,6 +0,0 @@ -from ultralytics import YOLO - - -model = YOLO("yolo11n-pose.pt") - -results = model.train(data="hand-keypoints.yaml", epochs=100, imgsz=640, device = "mps") diff --git a/object_detection/yolo11n-pose.pt b/object_detection/yolo11n-pose.pt deleted file mode 100644 index f8e7370..0000000 Binary files a/object_detection/yolo11n-pose.pt and /dev/null differ diff --git a/object_detection/yolo11n.pt b/object_detection/yolo11n.pt deleted file mode 100644 index 45b273b..0000000 Binary files a/object_detection/yolo11n.pt and /dev/null differ diff --git a/sim/MUJOCO_LOG.TXT b/sim/MUJOCO_LOG.TXT new file mode 100644 index 0000000..539f1b7 --- /dev/null +++ b/sim/MUJOCO_LOG.TXT @@ -0,0 +1,156 @@ +Sat Nov 1 14:16:16 2025 +WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 12.7480. + +Sat Nov 1 14:17:49 2025 +WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 12.7480. + +Sat Nov 1 15:20:20 2025 +WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 26.1140. + +Mon Nov 10 22:24:03 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 22:27:08 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 22:33:33 2025 +WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 22:43:02 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0020. + +Mon Nov 10 22:46:54 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0880. + +Mon Nov 10 22:52:32 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 5.9620. + +Mon Nov 10 22:55:47 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 22:56:04 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 22:58:18 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 22:59:54 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0940. + +Mon Nov 10 23:01:10 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0860. + +Mon Nov 10 23:01:50 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:06:35 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:06:52 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:09:48 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:13:43 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:14:55 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:15:42 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:16:41 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:18:52 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:19:37 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:22:40 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:24:09 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:24:59 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:25:18 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:25:54 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:27:10 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:30:26 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:32:49 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:33:02 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:39:15 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.1020. + +Mon Nov 10 23:39:40 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0860. + +Mon Nov 10 23:41:11 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:41:23 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:49:17 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:49:47 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:50:43 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:51:17 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:52:48 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Mon Nov 10 23:53:26 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Tue Nov 11 00:01:03 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0860. + +Tue Nov 11 00:01:51 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0860. + +Tue Nov 11 00:02:21 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0860. + +Tue Nov 11 00:02:55 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Tue Nov 11 00:03:04 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0860. + +Tue Nov 11 00:03:11 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0860. + +Tue Nov 11 00:03:20 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0860. + +Tue Nov 11 00:08:17 2025 +WARNING: Nan, Inf or huge value in QVEL at DOF 0. The simulation is unstable. Time = 0.0000. + +Tue Nov 11 00:10:47 2025 +WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 0.0000. + diff --git a/sim/balance_non_collidable_joint.xml b/sim/balance_non_collidable_joint.xml new file mode 100644 index 0000000..b94d362 --- /dev/null +++ b/sim/balance_non_collidable_joint.xml @@ -0,0 +1,79 @@ + + + diff --git a/sim/balanceball.xml b/sim/balanceball.xml new file mode 100644 index 0000000..42c0e03 --- /dev/null +++ b/sim/balanceball.xml @@ -0,0 +1,28 @@ + + \ No newline at end of file diff --git a/sim/balanceball_sim.py b/sim/balanceball_sim.py new file mode 100644 index 0000000..726b5e0 --- /dev/null +++ b/sim/balanceball_sim.py @@ -0,0 +1,211 @@ +from mujoco.glfw import glfw +import numpy as np +import mujoco as mj +import time +import os +import keyboard_control +# import sim.enums as states +from control_state import state + +# load model & set up data and camera +# Get the directory where this script is located +script_dir = os.path.dirname(os.path.abspath(__file__)) +modelPath = os.path.join(script_dir, "balance_non_collidable_joint.xml") +model = mj.MjModel.from_xml_path(modelPath) +body_id = mj.mj_name2id(model, mj.mjtObj.mjOBJ_BODY, "sphere_body") +head_id = mj.mj_name2id(model, mj.mjtObj.mjOBJ_BODY, "box_body") +data = mj.MjData(model) + +# visualization settings +cam = mj.MjvCamera() +cam.distance = 2 +cam.azimuth = 45 +cam.elevation = -35 +cam.orthographic = 1 + +opt = mj.MjvOption() + +data = mj.MjData(model) + +# define sphere joint +joint_id = mj.mj_name2id(model, mj.mjtObj.mjOBJ_JOINT, "sphere_free") +qpos_addr = model.jnt_qposadr[joint_id] # Position address (for free joint: [x, y, z, qw, qx, qy, qz]) +qvel_addr = model.jnt_dofadr[joint_id] # Velocity address (for free joint: [vx, vy, vz, wx, wy, wz]) + +# create sim window & set scene +glfw.init() +window = glfw.create_window(720,540,"Balanceball", None, None) +glfw.make_context_current(window) +mj.mjv_defaultOption(opt) +scene = mj.MjvScene(model, maxgeom=10000) +context = mj.MjrContext(model, mj.mjtFontScale.mjFONTSCALE_150.value) + +# set simulation timers +sim_dt = 0.001 +elapsed_time = 0.0 +time_prev = time.perf_counter() + +# set render timers +target_fps = 60 # You can change this if you want +frame_dt = 1.0/ target_fps +prev_render_t = time_prev + +key_states = { + 'W': False, # Forward perturbation (force +5 in x) + 'A': False, # Left perturbation (force +5 in y) + 'S': False, # Backward perturbation (force -5 in x) + 'D': False # Right perturbation (force -5 in y) +} + +def keyboard_callback(window, key, scancode, action, mods): + global key_states + + # Handle key press and release for WASD keys (perturbations only) + if action == glfw.PRESS: + if key == glfw.KEY_W: + key_states['W'] = True + elif key == glfw.KEY_A: + key_states['A'] = True + elif key == glfw.KEY_S: + key_states['S'] = True + elif key == glfw.KEY_D: + key_states['D'] = True + elif action == glfw.RELEASE: + if key == glfw.KEY_W: + key_states['W'] = False + elif key == glfw.KEY_A: + key_states['A'] = False + elif key == glfw.KEY_S: + key_states['S'] = False + elif key == glfw.KEY_D: + key_states['D'] = False + + +# PID gains for horizontal displacement control +Kp = 8.0 # Proportional gain +Kd = 1.5 # Derivative gain (damping) +KI = 1.0 # Integral gain +IMax = 0.3 # Maximum integral term + +# Velocity control parameters +max_velocity = 1.5 # Maximum desired velocity (m/s) +max_force = 25.0 # Maximum perturbation force (N) + +# PID controller for 2D horizontal error +def PID_2D(error_xy, prev_error_xy, total_sum_xy): + # error_xy is [error_x, error_y] + # Apply PID to each component + new_error_sum_x = np.clip(total_sum_xy[0] + error_xy[0] * sim_dt, -IMax, IMax) + new_error_sum_y = np.clip(total_sum_xy[1] + error_xy[1] * sim_dt, -IMax, IMax) + new_error_sum = np.array([new_error_sum_x, new_error_sum_y]) + + delta_error = error_xy - prev_error_xy + control_x = Kp * error_xy[0] + Kd * delta_error[0]/sim_dt + KI * new_error_sum[0] + control_y = Kp * error_xy[1] + Kd * delta_error[1]/sim_dt + KI * new_error_sum[1] + control = np.array([control_x, control_y]) + + return control, new_error_sum + +time_sum = 0 +# Initialize PID state variables (outside the loop so they persist) +prev_error_xy = np.array([0.0, 0.0]) # Previous horizontal error [x, y] +sum_error_xy = np.array([0.0, 0.0]) # Integral term [x, y] + +# Open window +while not glfw.window_should_close(window): + glfw.set_key_callback(window, keyboard_callback) + + # get current time + c_time = time.perf_counter() + frame_time = c_time - time_prev + time_prev = c_time + elapsed_time += frame_time + time_sum += frame_time + # simulation loop + while elapsed_time >= sim_dt: + + # Initialize force vector + force = np.zeros(6) + + # Body state: position of the center of the body + ball_pos = data.xpos[body_id].copy() + + # Head state: position of the head + head_pos = data.xpos[head_id].copy() + + # Compute horizontal error (head displacement from ball center in x-y plane) + # This is the key: we want to move the ball to reduce this horizontal displacement + error_xy = head_pos[:2] - ball_pos[:2] # [error_x, error_y] + error_xy[0] = round(error_xy[0], 3) + error_xy[1] = round(error_xy[1], 3) + print(error_xy) + # Get head velocity to predict future position (optional, helps with stability) + #head_vel = data.xvelp[head_id][:2] if hasattr(data, 'xvelp') else np.array([0.0, 0.0]) + + # Calculate desired velocities from PID control + # The PID controller outputs a desired velocity to reduce the horizontal error + # For balance ball: when head is ahead, move ball forward to get under it + control_xy, sum_error_xy = PID_2D(error_xy, prev_error_xy, sum_error_xy) + + # Update previous error for next iteration + prev_error_xy = error_xy.copy() + + # The control output is the desired velocity + # For balance ball: when head tilts forward (error_x > 0), we need to move ball backward + # to counteract the tilt and restore balance. The ball should move opposite to the error. + desired_vx = -control_xy[0] # Negative: move opposite to error direction + desired_vy = -control_xy[1] + + # Limit desired velocity to prevent instability + desired_vx = np.clip(desired_vx, -max_velocity, max_velocity) + desired_vy = np.clip(desired_vy, -max_velocity, max_velocity) + + # Apply direct velocity control + # Start control immediately to maintain balance + if time_sum >= 0.0: # Start control immediately + # Directly set the ball's velocity to the desired velocity + # For a free joint, qvel has [vx, vy, vz, wx, wy, wz] starting at qvel_addr + data.qvel[qvel_addr + 0] = desired_vx # vx + data.qvel[qvel_addr + 1] = desired_vy # vy + + # Apply perturbation forces (from WASD keys) - magnitude 5 in each direction + # These are applied as forces to test the balance controller's response + if key_states['W']: + force[0] += 10.0 # Forward perturbation (+x direction) + if key_states['S']: + force[0] += -10.0 # Backward perturbation (-x direction) + if key_states['A']: + force[1] += 10.0 # Left perturbation (+y direction) + if key_states['D']: + force[1] += -10.0 # Right perturbation (-y direction) + + + + # Limit perturbation forces + force[0] = np.clip(force[0], -max_force, max_force) + force[1] = np.clip(force[1], -max_force, max_force) + + # print("Error", error, "Control_x", control_vx, "Control_vy", control_vy, "Error_vector", vec) + # print("stopwatch ", time_sum) + + data.xfrc_applied[body_id] = force + mj.mj_step(model, data) + elapsed_time -= sim_dt + + + # Rendering Steps + if c_time - prev_render_t >= frame_dt: + prev_render_t = c_time + + cam.lookat[0:3] = data.qpos[qpos_addr : qpos_addr + 3] # [x, y, z] + viewport_width, viewport_height = glfw.get_framebuffer_size(window) + viewport = mj.MjrRect(0, 0, viewport_width, viewport_height) + mj.mjv_updateScene(model, data, opt, None, cam, mj.mjtCatBit.mjCAT_ALL.value, scene) + mj.mjr_render(viewport, scene, context) + glfw.swap_buffers(window) + glfw.poll_events() + + +glfw.terminate() + + diff --git a/sim/new_pid_sim/balanceball2.xml b/sim/new_pid_sim/balanceball2.xml new file mode 100644 index 0000000..fce45c6 --- /dev/null +++ b/sim/new_pid_sim/balanceball2.xml @@ -0,0 +1,81 @@ + + \ No newline at end of file diff --git a/sim/new_pid_sim/balanceball_sim2.py b/sim/new_pid_sim/balanceball_sim2.py new file mode 100644 index 0000000..1fbf832 --- /dev/null +++ b/sim/new_pid_sim/balanceball_sim2.py @@ -0,0 +1,204 @@ +from mujoco.glfw import glfw +import numpy as np +import mujoco as mj +import time + +import random + + + +# IMPORTANT DISCLAIMER: THIS WAS ALL WRITTEN BY CHATGPT I HAVE NO IDEA WHAT ITS DOING + + +# --------------------------- +# Model + Visualization setup +# --------------------------- +modelPath = "balanceball2.xml" +model = mj.MjModel.from_xml_path(modelPath) +body_id = mj.mj_name2id(model, mj.mjtObj.mjOBJ_BODY, "bb8") +head_id = mj.mj_name2id(model, mj.mjtObj.mjOBJ_BODY, "head") +data = mj.MjData(model) + +cam = mj.MjvCamera() +cam.distance = 2 +cam.azimuth = 45 +cam.elevation = -35 +cam.orthographic = 1 +opt = mj.MjvOption() + +# define bb8 joint +joint_id = mj.mj_name2id(model, mj.mjtObj.mjOBJ_JOINT, "bb8_free") +qpos_addr = model.jnt_qposadr[joint_id] + +# --------------------------- +# Window setup +# --------------------------- +glfw.init() +window = glfw.create_window(720, 540, "Balanceball", None, None) +glfw.make_context_current(window) +mj.mjv_defaultOption(opt) +scene = mj.MjvScene(model, maxgeom=10000) +context = mj.MjrContext(model, mj.mjtFontScale.mjFONTSCALE_150.value) + +# --------------------------- +# Timing +# --------------------------- +sim_dt = 0.001 +elapsed_time = 0.0 +time_prev = time.perf_counter() +target_fps = 60 +frame_dt = 1.0 / target_fps +prev_render_t = time_prev + +# --------------------------- +# Keyboard state +# --------------------------- +key_states = {k: False for k in ['W','A','S','D','1','2','3','4']} + +def keyboard_callback(window, key, scancode, action, mods): + if key in (glfw.KEY_W, glfw.KEY_A, glfw.KEY_S, glfw.KEY_D, + glfw.KEY_1, glfw.KEY_2, glfw.KEY_3, glfw.KEY_4): + pressed = (action == glfw.PRESS or action == glfw.REPEAT) + if key == glfw.KEY_W: key_states['W'] = pressed + elif key == glfw.KEY_A: key_states['A'] = pressed + elif key == glfw.KEY_S: key_states['S'] = pressed + elif key == glfw.KEY_D: key_states['D'] = pressed + elif key == glfw.KEY_1: key_states['1'] = pressed + elif key == glfw.KEY_2: key_states['2'] = pressed + elif key == glfw.KEY_3: key_states['3'] = pressed + elif key == glfw.KEY_4: key_states['4'] = pressed + +glfw.set_key_callback(window, keyboard_callback) + +# --------------------------- +# PID parameters +# --------------------------- +Kp = 5 +Ki = 0.3 +Kd = 0.2 +I_MAX = 0.3 + +# desired roll-rate cap (interpreted as angular speed command proxy) +OMEGA_MAX = 4 + +# --------------------------- +# PID controller +# --------------------------- +def PID(error, prev_error, int_error, Kp, Ki, Kd, dt, I_MAX): + d_error = (error - prev_error) / dt + new_int = np.clip(int_error + error * dt, -I_MAX, I_MAX) + control = -(Kp * error + Ki * new_int + Kd * d_error) + return control, new_int + +# --------------------------- +# Actuator mixing (3 motors @ 0°, 120°, 240°) +# Each motor command = [cosθ, sinθ] · [cmd_x, cmd_y] +# If a motor spins the opposite way physically, flip its sign in MOTOR_SIGN. +# --------------------------- +# angles = np.array([0.0, np.pi/2, np.pi, 3*np.pi/2]) # front, right, back, left +# MIX = np.vstack((np.cos(angles), np.sin(angles))).T # shape (4,2) +# MOTOR_SIGN = np.array([1.0, 1.0, 1.0, 1.0]) # tweak signs if any motor is reversed +# # Per-actuator ctrlrange from XML for safe clamping: + +angles = np.array([0.0, + 2*np.pi/3, + 4*np.pi/3]) # 0°, 120°, 240° +MIX = np.vstack((np.cos(angles), np.sin(angles))).T # shape (3,2) +MOTOR_SIGN = np.array([1.0, 1.0, 1.0]) # adjust if needed + +# Per-actuator ctrlrange from XML: +CTRL_MIN = model.actuator_ctrlrange[:3, 0] +CTRL_MAX = model.actuator_ctrlrange[:3, 1] + + +# --------------------------- +# Main loop +# --------------------------- +prev_error = np.zeros(2) +int_error = np.zeros(2) +time_sum = 0.0 + +# Random perturbation scheduling +next_perturb_time = 0.5 +perturb_duration = 0.4 +active_perturb_end = 0.0 +force_mag = 50.0 + +while not glfw.window_should_close(window): + + c_time = time.perf_counter() + frame_time = c_time - time_prev + time_prev = c_time + elapsed_time += frame_time + time_sum += frame_time + + while elapsed_time >= sim_dt: + + # --- Get positions --- + ball_pos = data.xpos[body_id].copy() + head_pos = data.xpos[head_id].copy() + + # --- Compute horizontal error (head above ball center) --- + error_xy = head_pos[:2] - ball_pos[:2] + + # --- PID → desired roll vector (cmd_x, cmd_y) --- + + c_time = time.perf_counter() + frame_time = c_time - time_prev + time_prev = c_time + elapsed_time += frame_time + time_sum += frame_time + cmd_xy = np.zeros(2) + if time_sum >= 1.0: + cmd_xy, int_error = PID(error_xy, prev_error, int_error, Kp, Ki, Kd, sim_dt, I_MAX) + prev_error = error_xy + + # cap desired roll rate amplitude + norm = np.linalg.norm(cmd_xy) + if norm > OMEGA_MAX: + cmd_xy *= (OMEGA_MAX / (norm + 1e-9)) + + # keyboard nudges (adds to desired roll vector) + if key_states['W']: cmd_xy[0] += 0.5 + if key_states['S']: cmd_xy[0] -= 0.5 + if key_states['A']: cmd_xy[1] += 0.5 + if key_states['D']: cmd_xy[1] -= 0.5 + + # --- Mix to 3 motors --- + raw_ctrl = (MIX @ cmd_xy) * MOTOR_SIGN # shape (3,) + data.ctrl[0:3] = np.minimum(np.maximum(raw_ctrl, CTRL_MIN), CTRL_MAX) + + # --- Perturbation test forces --- + force = np.zeros(6) + if 0.5 <= time_sum <= 1.0: + force[0] = 5.0 # initial shove + + if time_sum >= next_perturb_time: + direction = random.choice([(1, 0), (-1, 0), (0, 1), (0, -1)]) + force[0] = direction[0] * force_mag + force[1] = direction[1] * force_mag + active_perturb_end = time_sum + perturb_duration + next_perturb_time = time_sum + random.uniform(1.0, 5.0) + force_mag = random.randint(25,100) + print(f"Random push {force_mag} at {time_sum:.2f}s: dir={direction}") + + if time_sum < active_perturb_end: + data.xfrc_applied[body_id] = force + else: + data.xfrc_applied[body_id, :] = 0.0 + + mj.mj_step(model, data) + elapsed_time -= sim_dt + + # --- Rendering --- + if c_time - prev_render_t >= frame_dt: + prev_render_t = c_time + cam.lookat[0:3] = data.qpos[qpos_addr : qpos_addr + 3] + w, h = glfw.get_framebuffer_size(window) + viewport = mj.MjrRect(0, 0, w, h) + mj.mjv_updateScene(model, data, opt, None, cam, mj.mjtCatBit.mjCAT_ALL.value, scene) + mj.mjr_render(viewport, scene, context) + glfw.swap_buffers(window) + glfw.poll_events() + +glfw.terminate() diff --git a/sim/pendulum_viewer.py b/sim/pendulum_viewer.py new file mode 100644 index 0000000..b646795 --- /dev/null +++ b/sim/pendulum_viewer.py @@ -0,0 +1,82 @@ +import mujoco as mj +from mujoco.glfw import glfw +import numpy as np + +# This viewer demonstrates: +# - Loading a MuJoCo model from XML (pendulum.xml) +# - Using keyboard to apply torque to the pendulum +# - Accessing joint/actuator IDs and simulation data (qpos, qvel) +# - Printing and visualizing state for workshop learning + +modelPath = 'pendulum.xml' +displayRefreshRate = 60 +torque = 0.0 + +def keyboard_callback(window, key, scancode, action, mods): + global torque + if action == glfw.PRESS or action == glfw.REPEAT: + if key == glfw.KEY_LEFT: + torque = -1.0 # Apply negative torque + elif key == glfw.KEY_RIGHT: + torque = 1.0 # Apply positive torque + elif key == glfw.KEY_SPACE: + torque = 0.0 # Stop torque + elif action == glfw.RELEASE: + if key in [glfw.KEY_LEFT, glfw.KEY_RIGHT]: + torque = 0.0 + +# Load model and data +model = mj.MjModel.from_xml_path(modelPath) +data = mj.MjData(model) +cam = mj.MjvCamera() +opt = mj.MjvOption() + +# Get joint and actuator IDs by name (from XML) +joint_name = model.joint(0).name if model.njnt > 0 else None +joint_id = mj.mj_name2id(model, mj.mjtObj.mjOBJ_JOINT, joint_name) if joint_name else -1 +actuator_name = model.actuator(0).name if model.nu > 0 else None +actuator_id = mj.mj_name2id(model, mj.mjtObj.mjOBJ_ACTUATOR, actuator_name) if actuator_name else -1 + +print(f"Loaded pendulum.xml. Joint name: {joint_name}, id: {joint_id}") +print(f"Actuator name: {actuator_name}, id: {actuator_id}") + +# Initialize GLFW and window +if not glfw.init(): + raise RuntimeError('Could not initialize GLFW') +window = glfw.create_window(600, 400, "Pendulum Viewer", None, None) +glfw.make_context_current(window) +glfw.set_key_callback(window, keyboard_callback) + +mj.mjv_defaultCamera(cam) +mj.mjv_defaultOption(opt) +scene = mj.MjvScene(model, maxgeom=1000) +context = mj.MjrContext(model, mj.mjtFontScale.mjFONTSCALE_150.value) + +cam.distance = 2 +cam.elevation = -20 +cam.azimuth = 90 + +while not glfw.window_should_close(window): + # Apply torque to actuator (if present) + if actuator_id != -1: + data.ctrl[actuator_id] = torque + + # Step simulation + mj.mj_step(model, data) + + # Print qpos and qvel for the pendulum joint + if joint_id != -1: + qpos = data.qpos[model.jnt_qposadr[joint_id]] + qvel = data.qvel[model.jnt_dofadr[joint_id]] + print(f"qpos (angle): {qpos:.3f}, qvel (angular vel): {qvel:.3f}", end='\r') + + # Render + viewport_width, viewport_height = glfw.get_framebuffer_size(window) + viewport = mj.MjrRect(0, 0, viewport_width, viewport_height) + cam.lookat[:] = 0 # Center camera + mj.mjv_updateScene(model, data, opt, None, cam, mj.mjtCatBit.mjCAT_ALL.value, scene) + mj.mjr_render(viewport, scene, context) + glfw.swap_buffers(window) + glfw.poll_events() + +glfw.terminate() diff --git a/sim/sim_templates/mujoco_test.py b/sim/sim_templates/mujoco_test.py index 2808ba4..a804903 100644 --- a/sim/sim_templates/mujoco_test.py +++ b/sim/sim_templates/mujoco_test.py @@ -1,6 +1,7 @@ import mujoco as mj from mujoco.glfw import glfw import numpy as np +import os """ Motor Control System for BB8 Robot @@ -44,7 +45,10 @@ print_camera_config = 1 #set to 1 to print camera config #this is useful for initializing view of the model -modelPath = "sim/test_world.xml" +# Get the directory where this script is located, then go up one level to sim/ +script_dir = os.path.dirname(os.path.abspath(__file__)) +sim_dir = os.path.dirname(script_dir) # Go up from sim_templates to sim/ +modelPath = os.path.join(sim_dir, "balance_non_collidable_joint.xml") displayRefreshRate = 60 @@ -154,13 +158,13 @@ def keyboard_callback(window, key, scancode, action, mods): -sphere = mj.mj_name2id(model, mj.mjtObj.mjOBJ_BODY, "bb8") +sphere_body_id = mj.mj_name2id(model, mj.mjtObj.mjOBJ_BODY, "sphere_body") debug = False # Init GLFW, create window, make OpenGL context current, request v-sync glfw.init() -window = glfw.create_window(int(1200*0.6), int(900*0.6), "Quadruped", None, None) +window = glfw.create_window(int(1200*0.6), int(900*0.6), "Balance Ball", None, None) glfw.make_context_current(window) # glfw.swap_interval(1) @@ -179,13 +183,14 @@ def keyboard_callback(window, key, scancode, action, mods): cam.elevation = -35 cam.orthographic = 1 -# sphere_address = model.jnt_qposadr[sphere] # Get qpos index for the cube joint - -joint_id = mj.mj_name2id(model, mj.mjtObj.mjOBJ_JOINT, "bb8_free") +# Get qpos address for the sphere_free joint +joint_id = mj.mj_name2id(model, mj.mjtObj.mjOBJ_JOINT, "sphere_free") qpos_addr = model.jnt_qposadr[joint_id] -head_id = mj.mj_name2id(model, mj.mjtObj.mjOBJ_BODY, "head") -# head_site_id = mj.mj_name2id(model, mj.mjtObj.mjOBJ_SITE, "head_site") +box_body_id = mj.mj_name2id(model, mj.mjtObj.mjOBJ_BODY, "box_body") + +# Get sensor ID for the gyro +gyro_top_id = mj.mj_name2id(model, mj.mjtObj.mjOBJ_SENSOR, "box_weight_gyro_top") # Define the four tilted motor axes (fixed relative to world frame) # Two axes in x-z plane, 45° from z-axis @@ -425,26 +430,25 @@ def calculate_and_print_rotation_axis(): cameraControl = 'none' print("Camera reset to default position") - # Get the site ID once (do this outside the loop) - - # # Inside your render loop: - bb8_pos = data.qpos[qpos_addr : qpos_addr + 3] # ball's world position - new_head_pos = bb8_pos + np.array([0.0, 0.0, 0.01]) # 0.01m above ball center - # # data.qpos[head_id] = new_head_pos[0] - - # # Use MuJoCo API to set body position - # mj.mj_set_body_xpos(model, data, head_id, new_head_pos) - - head_bid = mj.mj_name2id(model, mj.mjtObj.mjOBJ_BODY, "head") - data.mocap_pos[head_bid] = new_head_pos + # Get sphere position (the sphere has a free joint, so its position is in qpos) + sphere_pos = data.qpos[qpos_addr : qpos_addr + 3] # sphere's world position - # MuJoCo doesn't let you directly set site_xpos, so you need to: - # (1) define a mocap body, or - # (2) draw a marker at the updated position each frame - # (3) or manually move the site body using a 'head' body instead + # Note: The box_body is a child of link_body, which is constrained to sphere_center + # via equality constraint, so it should move automatically with the sphere. + # No need to manually position it. # Step the MuJoCo simulation mj.mj_step(model, data) + + # Get and print gyro data + # Gyro sensor returns 3 values: [wx, wy, wz] (angular velocity in rad/s) + # Use sensor address to get the correct location in sensordata array + gyro_top_addr = model.sensor_adr[gyro_top_id] + + gyro_top_data = data.sensordata[gyro_top_addr:gyro_top_addr + 3] + + print(f"Gyro Top: [{gyro_top_data[0]:.4f}, {gyro_top_data[1]:.4f}, {gyro_top_data[2]:.4f}] rad/s") + glfw.poll_events() @@ -453,8 +457,9 @@ def calculate_and_print_rotation_axis(): viewport_width, viewport_height = glfw.get_framebuffer_size(window) viewport = mj.MjrRect(0, 0, viewport_width, viewport_height) - # Camera updates (tracking robot position) - cam.lookat[0:3] = data.qpos[qpos_addr : qpos_addr + 3] # [x, y, z] + # Camera updates (tracking sphere position) + sphere_pos = data.qpos[qpos_addr : qpos_addr + 3] # sphere's world position + cam.lookat[0:3] = sphere_pos # [x, y, z] # Move first light to 2m above the sphere # model.light_pos[0][:3] = data.qpos[qpos_addr : qpos_addr + 3] + np.array([0.0, 0.0, 2.0]) diff --git a/sim/simple_viewer.py b/sim/simple_viewer.py new file mode 100644 index 0000000..525b230 --- /dev/null +++ b/sim/simple_viewer.py @@ -0,0 +1,13 @@ +import mujoco +import mujoco.viewer +import numpy as np +import time + +model = mujoco.MjModel.from_xml_path("balanceball.xml") +data = mujoco.MjData(model) +mujoco.mj_resetData(model, data) + +with mujoco.viewer.launch_passive(model, data) as viewer: + while viewer.is_running(): + mujoco.mj_step(model, data) + viewer.sync() \ No newline at end of file