import numpy as np
from hold import Hold
from wall import Wall
from climber import Climber
from torso import Torso
from state import State
from planner import find_best_betasholds = [ Hold(0.0, 0.0, 0.0), Hold(50, 30, 0.0), Hold(-50, 40, 0.0), Hold(20, 80, 0.0), Hold(-30, 90, 0.0), Hold(60, 120, 0.0), Hold(-50, 130, 0.0), Hold(0.0, 160, 0.0)]
wall = Wall(holds)climber = Climber(height=170)climber.mass = 65torso = Torso( position=np.array([0.0, 5.0, 15.0]), roll=0, pitch=0, yaw=0, width=40, height=60, thickness=20)state = State( left_hand=2, right_hand=1, left_foot=0, right_foot=0, torso=torso)roll_values = [-10, 0, 10]pitch_values = [-10, 0, 10]yaw_values = [-10, 0, 10]arm_extensions = [0.7, 0.8, 0.9, 1.0]leg_extensions = [0.7, 0.8, 0.9, 1.0]betas = find_best_betas( state, wall, climber, radius=80, roll_values=roll_values, pitch_values=pitch_values, yaw_values=yaw_values, arm_extensions=arm_extensions, leg_extensions=leg_extensions)print(f"Found {len(betas)} betas")for i, beta in enumerate(betas, start=1): print(f"\n===== BETA {i} =====") print(f"Score: {beta.score}") print(f"Left Hand : {beta.left_hand}") print(f"Right Hand: {beta.right_hand}") print(f"Left Foot : {beta.left_foot}") print(f"Right Foot: {beta.right_foot}") print(f"Roll : {beta.torso.roll}") print(f"Pitch: {beta.torso.pitch}") print(f"Yaw : {beta.torso.yaw}") print(f"Left Arm Margin : {beta.left_arm_margin:.2f}") print(f"Right Arm Margin: {beta.right_arm_margin:.2f}") print(f"Left Leg Margin : {beta.left_leg_margin:.2f}") print(f"Right Leg Margin: {beta.right_leg_margin:.2f}")```
Comments 0
No comments yet. Be the first!
Sign in to join the conversation.