Climbing Beta Optimizer

Working on a new pipeline!
Input –> Wall Reconstruction      (holds, wall angle, topology) –>   Initial Climber State       (current stance + body model) –>  Context Analyzer (Cheap)         • Hold distances      • Wall angle      • Reach estimate      • Body orientation      • Route direction –>  Relevant Movement Primitives –> Static Reach Generator              Dyno Generator      Match Generator     Foot Swap Generator    Cross Through Generator  Heel Hook Generator       –> Generate transitions to get to that specific stance –> Lazy A* search –> Inverse Kinematics finds torso orientation (roll, pitch, yaw etc) –> Statics resolves forces to determine if it is feasible without falling –> Cost evaluation scores the routes –> Optimal beta returned

from models.climber import Climber
from models.pose import Pose
from models.stance import Stance
from models.wall 
import Wall
class IKSolver:    def solve(        self,        stance: Stance,        wall: Wall,        climber: Climber,    ) -> Pose | None:             
 # ----------------------------        
# Retrieve holds        
# ----------------------------       
 lh = wall.get_hold(stance.left_hand)        
rh = wall.get_hold(stance.right_hand)        
lf = wall.get_hold(stance.left_foot)        
rf = wall.get_hold(stance.right_foot)        
# ----------------------------        
# Initial torso estimate        
# ----------------------------       
 torso_x = (lh.x + rh.x + lf.x + rf.x) / 4       
 torso_y = (lh.y + rh.y + lf.y + rf.y) / 4        
torso_z = (lh.z + rh.z + lf.z + rf.z) / 4       
 # ----------------------------        
# Reach limits        
# ----------------------------        
arm_reach = climber.upper_arm + climber.forearm        leg_reach = climber.thigh + climber.shin        
# Approximate shoulder locations        
left_shoulder = (            torso_x - climber.shoulder_width / 2,            torso_y,            torso_z,        )        
right_shoulder = (            torso_x + climber.shoulder_width / 2,            torso_y,            torso_z,        )        
# Approximate hip locations        
left_hip = (            torso_x - climber.hip_width / 2,            torso_y,            torso_z,        )        
right_hip = (            torso_x + climber.hip_width / 2,            torso_y,            torso_z,        )       
 # ----------------------------        
# Distance helper       
 # ----------------------------        
def distance(a, b):            return sqrt(                (a[0] - b[0]) ** 2                + (a[1] - b[1]) ** 2                + (a[2] - b[2]) ** 2            )        
# ----------------------------        
# Check each limb       
 # ----------------------------        
if distance(left_shoulder, (lh.x, lh.y, lh.z)) > arm_reach:            return None        
if distance(right_shoulder, (rh.x, rh.y, rh.z)) > arm_reach:            return None        
if distance(left_hip, (lf.x, lf.y, lf.z)) > leg_reach:            return None        
if distance(right_hip, (rf.x, rf.y, rf.z)) > leg_reach:            return None        
# ----------------------------        
# Valid pose        
# ----------------------------        
return Pose(            torso_x=torso_x,            torso_y=torso_y,            torso_z=torso_z,            roll=0.0,            pitch=0.0,            yaw=0.0,        )```

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}")```