diff --git a/app/main.py b/app/main.py index bc28f41..84963a2 100644 --- a/app/main.py +++ b/app/main.py @@ -1,32 +1,26 @@ import harfang as hg import requests import socket -from math import pi from statistics import median import math from OrbitalCam import OrbitalController +from enum import Enum str_error = "The robot is not connected" # helpers - - def clamp(v, v_min, v_max): return max(v_min, min(v, v_max)) - def rangeadjust_clamp(k, a, b, u, v): return rangeadjust(clamp(k, a, b), a, b, u, v) - def rangeadjust(k, a, b, u, v): return (k - a) / (b - a) * (v - u) + u - def lerp(k, a, b): return a + (b - a) * k - # look for the poppy on the network url = "" try: @@ -42,13 +36,14 @@ def lerp(k, a, b): mouse = hg.Mouse() res_x, res_y = 1920, 1080 - +WIDTH, HEIGHT = res_x, res_y # initialize lists and variables for toggle button (swipe style) mousexlist = [] mouseylist = [] has_switched = False compliance_mode = False compliance_lerp = True +ui_interaction_mode = False win = hg.NewWindow("Harfang - Poppy", res_x, res_y, 32) # , hg.WV_Fullscreen) @@ -56,7 +51,7 @@ def lerp(k, a, b): hg.RenderReset(res_x, res_y, hg.RF_MSAA8X | hg.RF_FlipAfterRender | hg.RF_FlushAfterRender | hg.RF_MaxAnisotropy) -hg.AddAssetsFolder("resources_compiled") +hg.AddAssetsFolder("../resources_compiled") # AAA render params aaa_config = hg.ForwardPipelineAAAConfig() @@ -178,15 +173,17 @@ def lerp(k, a, b): app_clock = 0 app_status = "dancing" - def toggle_button(label, value, x, y): global has_switched + + #Target for user interaction mat = hg.TransformationMat4( hg.Vec3(x, y, 1), hg.Vec3(0, 0, 0), hg.Vec3(1, 1, 1)) pos = hg.GetT(mat) axis_x = hg.GetX(mat) * 56 axis_y = hg.GetY(mat) * 24 + #Rectangular zone drawn on scren who depends on var "value" toggle_vtx = hg.Vertices(vtx_layout, 4) toggle_vtx.Begin(0).SetPos( pos - axis_x - axis_y).SetTexCoord0(hg.Vec2(0, 1)).End() @@ -199,8 +196,9 @@ def toggle_button(label, value, x, y): toggle_idx = [0, 3, 2, 0, 2, 1] hg.DrawTriangles(view_id, toggle_idx, toggle_vtx, shader_for_plane, [], [ - texture_on if value else texture_off], render_state_quad) + texture_on if value else texture_off], render_state_quad) + #if player click on interactive zone if mouse.Down(hg.MB_0): mousexlist.append(mouse.X()) mouseylist.append(mouse.Y()) @@ -209,6 +207,7 @@ def toggle_button(label, value, x, y): mouseylist.clear() has_switched = False + #Then code verify if player clicked many time to get median value from mouse positions if len(mousexlist) > 20: mousexlist.pop(0) if len(mouseylist) > 20: @@ -217,12 +216,15 @@ def toggle_button(label, value, x, y): if len(mouseylist) > 0: mouse_x = median(mousexlist) mouse_y = median(mouseylist) + + #if mouse is in rectancle zone defined by our vertices if mouse_x > pos.x - axis_x.x and mouse_x < pos.x + axis_x.x and mouse_y > pos.y - axis_y.y and mouse_y < pos.y + axis_y.y and not has_switched: value = True if not value else False has_switched = True mousexlist.clear() mouseylist.clear() + #This is the text asociated to the interactive zone next to it mat = hg.TranslationMat4(hg.Vec3(pos.x + axis_x.x + 10, y - 10, 1)) hg.SetS(mat, hg.Vec3(1, -1, 1)) hg.DrawText(view_id, @@ -232,10 +234,8 @@ def toggle_button(label, value, x, y): [font_color_white], [], text_render_state) return value - buttonlist = [[100, res_y - 80]] - def is_switching(): for i in buttonlist: mat = hg.TransformationMat4( @@ -318,32 +318,88 @@ def get_v_from_dancing(id_robot): send_dt = 1/10 # send information to the poppy every send_dt # get values from the real robot + + if ui_interaction_mode: + compliance_mode = False + app_status = "None" + + if mouse.Down(hg.MB_0): + mousexlist.append(mouse.X()) + mouseylist.append(mouse.Y()) + else: + mousexlist.clear() + mouseylist.clear() + has_switched = False + + if len(mousexlist) > 20: + mousexlist.pop(0) + if len(mouseylist) > 20: + mouseylist.pop(0) + + if len(mouseylist) > 0: + mouse_x = median(mousexlist) + mouse_y = median(mouseylist) + + Sx = 0.0 + Sy = 0.0 + + Ex = WIDTH + + interactable_rectangles = [] + + for i in range(1, 7): + + Ey = (HEIGHT / 6) * i + + rectangle = hg.Rect(Sx, Sy, Ex, Ey) + interactable_rectangles.append(rectangle) + + Sy = Ey + + + for i in range(len(interactable_rectangles)): + inter_sx = interactable_rectangles[i].sx + inter_sy = interactable_rectangles[i].sy + inter_ex = interactable_rectangles[i].ex + inter_ey = interactable_rectangles[i].ey + + if mouse.Down(hg.MB_0): + if mouse_x > inter_sx and mouse_x < inter_ex and mouse_y > inter_sy and mouse_y < inter_ey: + hg_m = hg_motors[i] + new_motor_angle = rangeadjust(mouse_x, 0, WIDTH, hg_m["lower_limit"], hg_m["upper_limit"]) #rename + hg_m["v"] = new_motor_angle + + if not compliance_mode and not ui_interaction_mode: + app_status = "dancing" if compliance_mode: timer_requests_not_overload += hg.time_to_sec_f(dt) - if timer_requests_not_overload > send_dt: - timer_requests_not_overload = 0 + ui_interaction_mode = False + if timer_requests_not_overload > send_dt: + timer_requests_not_overload = 0 - if url != "": - try: - r = requests.get(url + "/motors/get/positions").text - for id, m in enumerate(hg_motors): - hg_m = hg_motors[id] - hg_motors_previous[id][0] = hg_m["v"] - hg_motors_previous[id][1] = hg.GetClock() - hg_m["v"] = float(r.split(';')[id]) - if id == 5: - # send negative value for claw motor angle () - hg_m["v"] = -float(r.split(';')[id]) + if url != "": + try: + r = requests.get(url + "/motors/get/positions").text + for id, m in enumerate(hg_motors): + hg_m = hg_motors[id] + hg_motors_previous[id][0] = hg_m["v"] + hg_motors_previous[id][1] = hg.GetClock() + hg_m["v"] = float(r.split(';')[id]) + if id == 5: + # send negative value for claw motor angle () + hg_m["v"] = -float(r.split(';')[id]) - except: - print("Robot not connected " + url) + except: + print("Robot not connected " + url) # set 3D mesh with the current motor pos + + v = 0 for id, m in enumerate(hg_motors): hg_m = hg_motors[id] # check if we are getting value from the real root - if compliance_mode: + if compliance_mode or ui_interaction_mode: v = hg_m["v"] else: # sending value to the real robot if app_status == "dancing": @@ -375,22 +431,22 @@ def get_v_from_dancing(id_robot): # set the position to the virtual robot rot = hg.Vec3(0, 0, 0) if hg_m["axis"] == "X": - rot = hg.Vec3(-new_v*pi/180.0, 0, 0) + rot = hg.Vec3(-new_v*math.pi/180.0, 0, 0) elif hg_m["axis"] == "Y": - rot = hg.Vec3(0, -new_v*pi/180.0, 0) + rot = hg.Vec3(0, -new_v*math.pi/180.0, 0) elif hg_m["axis"] == "Z": - rot = hg.Vec3(-1.57, 0, -new_v*pi/180.0) + rot = hg.Vec3(-1.57, 0, -new_v*math.pi/180.0) hg_m["n"].GetTransform().SetRot(rot) else: rot = hg.Vec3(0, 0, 0) if hg_m["axis"] == "X": - rot = hg.Vec3(-v*pi/180.0, 0, 0) + rot = hg.Vec3(-v*math.pi/180.0, 0, 0) elif hg_m["axis"] == "Y": - rot = hg.Vec3(0, -v*pi/180.0, 0) + rot = hg.Vec3(0, -v*math.pi/180.0, 0) elif hg_m["axis"] == "Z": - rot = hg.Vec3(-1.57, 0, -v*pi/180.0) + rot = hg.Vec3(-1.57, 0, -v*math.pi/180.0) hg_m["n"].GetTransform().SetRot(rot) @@ -573,6 +629,8 @@ def get_v_from_dancing(id_robot): if compliance_mode: compliance_lerp = toggle_button( "Motion Interpolation ON" if compliance_lerp else "Motion Interpolation OFF", compliance_lerp, 100, res_y - 180) + ui_interaction_mode = toggle_button( + "Interaction Mode ON" if ui_interaction_mode else "Interaction Mode OFF", ui_interaction_mode, 100, res_y - 280) view_id += 1