Merge pull request #5 from disketflu/master

Added multi-device functional toggle button for compliance mode

app/OrbitalCam.py

@@ -37,7 +37,6 @@ def OrbitalController(keyboard, mouse, cam_pos, cam_rot, cam_tgt, dt, width, hei
 	smoothed_dx += (delta_x - smoothed_dx) * 0.1
 	smoothed_dy += (delta_y - smoothed_dy) * 0.1
 
-	state_modified = False
 	speed = dt_sec * cam_rot_speed
 	cam_rot.x += smoothed_dy * speed
 	cam_rot.y += smoothed_dx * speed

app/poppy_api_rest.py

@@ -2,6 +2,7 @@
 import requests
 import socket
 from math import pi
+from statistics import median
 import math
 from OrbitalCam import OrbitalController
 
@@ -39,6 +40,12 @@ def lerp(k, a, b):
 
 res_x, res_y = 1920, 1080
 
+#initialize lists and variables for toggle button (swipe style)
+mousexlist = []
+mouseylist = []
+has_switched = False
+compliance_mode = False
+
 win = hg.NewWindow("Harfang - Poppy", res_x, res_y, 32)#, hg.WV_Fullscreen)
 hg.RenderInit(win)
 hg.RenderReset(res_x, res_y, hg.RF_MSAA8X | hg.RF_FlipAfterRender | hg.RF_FlushAfterRender | hg.RF_MaxAnisotropy)
@@ -71,6 +78,12 @@ def lerp(k, a, b):
 target_tex = hg.LoadTextureFromAssets("Asset_2.png", 0)[0]
 texture_asset2 = hg.MakeUniformSetTexture("s_texTexture", target_tex, 0)
 
+target_tex = hg.LoadTextureFromAssets("switch-on.png", 0)[0]
+texture_on = hg.MakeUniformSetTexture("s_texTexture", target_tex, 0)
+
+target_tex = hg.LoadTextureFromAssets("switch-off.png", 0)[0]
+texture_off = hg.MakeUniformSetTexture("s_texTexture", target_tex, 0)
+
 
 # load shaders
 render_state_quad = hg.ComputeRenderState(hg.BM_Alpha, hg.DT_Less, hg.FC_Disabled)
@@ -143,8 +156,68 @@ def lerp(k, a, b):
 hg.ImGuiInit(10, imgui_prg, imgui_img_prg)
 
 app_clock = 0
+app_status = "dancing"
+
+def toggle_button(label, value, x, y):
+	global has_switched
+	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
+
+	toggle_vtx = hg.Vertices(vtx_layout, 4)
+	toggle_vtx.Begin(0).SetPos(pos - axis_x - axis_y).SetTexCoord0(hg.Vec2(0, 1)).End()
+	toggle_vtx.Begin(1).SetPos(pos - axis_x + axis_y).SetTexCoord0(hg.Vec2(0, 0)).End()
+	toggle_vtx.Begin(2).SetPos(pos + axis_x + axis_y).SetTexCoord0(hg.Vec2(1, 0)).End()
+	toggle_vtx.Begin(3).SetPos(pos + axis_x - axis_y).SetTexCoord0(hg.Vec2(1, 1)).End()
+	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)
+
+	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)
+		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()
+
+	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,
+				font,
+				label, shader_font, "u_tex", 0,
+				mat, hg.Vec3(0, 0, 0), hg.DTHA_Left, hg.DTVA_Top,
+				[font_color_white], [], text_render_state)
+	return value
+
+buttonlist = [[100, res_y - 80]]
+
+def is_switching():
+	for i in buttonlist:
+		mat = hg.TransformationMat4(hg.Vec3(i[0], i[1], 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
+		if (mouse.X() > pos.x - axis_x.x - 10 and mouse.X() < pos.x + axis_x.x + 10 and mouse.Y() > pos.y - axis_y.y - 10 and mouse.Y() < pos.y + axis_y.y + 10 and mouse.Down(hg.MB_0)): # check if mouse is clicked and is within the button area + 10px on every side
+				return True
+	return False
+
 def get_v_from_dancing(id_robot):
 	elapsed_time = app_clock *0.5
 	_amp = 1
@@ -193,7 +266,7 @@ def get_v_from_dancing(id_robot):
 	render_was_reset, res_x, res_y = hg.RenderResetToWindow(win, res_x, res_y, hg.RF_VSync | hg.RF_MSAA4X | hg.RF_MaxAnisotropy)
 	res_y = max(res_y, 16)
 
-	if hg.ImGuiIsWindowFocused():
+	if hg.ImGuiIsWindowFocused() and not is_switching():
 		world, cam_rot, cam_tgt, cam_pos = OrbitalController(keyboard, mouse, cam_pos, cam_rot, cam_tgt, dt, res_x, res_y)
 		cam.GetTransform().SetWorld(world)
 
@@ -209,23 +282,46 @@ def get_v_from_dancing(id_robot):
 	url_set_motor_pos = url + "/motors/set/goto/"
 	send_dt = 1/10 # send information to the poppy every send_dt
 
+	# get values from the real robot
+	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
+
+			if url != "":
+				try:
+					r = requests.get(url + "/motors/get/positions").text
+					for id, m in enumerate(hg_motors):
+						hg_m = hg_motors[id]
+						hg_m["v"] = float(r.split(';')[id])
+						if id==5: hg_m["v"] = -float(r.split(';')[id])	# send negative value for claw motor angle ()
+
+				except:
+					print("Robot not connected " + url)
+
 	# set 3D mesh with the current motor pos
 	for id, m in enumerate(hg_motors):
-		v = get_v_from_dancing(id)
 		hg_m = hg_motors[id]
 
+		# check if we are getting value from the real root
+		if compliance_mode:
+			v = hg_m["v"]
+		else:  # sending value to the real robot
+			if app_status == "dancing":
+				v = get_v_from_dancing(id)
+
 		# led
 		led_actual_color = "off"
 		if led_current_motor_ping_pong == id:
 			led_actual_color = led_colors[led_color]
 
 		# send the next position to the robot
 		if id == 5:
-			url_set_motor_pos_registers += f"m{id + 1}:compliant:False;m{id + 1}:led:{led_actual_color};"
-			url_set_motor_pos += f"m{id+1}:{-v}:{send_dt};"
+			url_set_motor_pos_registers += f"m{id + 1}:compliant:{'True' if compliance_mode else 'False'};m{id + 1}:led:{led_actual_color};"
+			url_set_motor_pos += f"m{id + 1}:{-v}:{send_dt};"
 		else:
-			url_set_motor_pos_registers += f"m{id + 1}:compliant:False;m{id + 1}:led:{led_actual_color};"
-			url_set_motor_pos += f"m{id+1}:{v}:{send_dt};"
+			url_set_motor_pos_registers += f"m{id + 1}:compliant:{'True' if compliance_mode else 'False'};m{id + 1}:led:{led_actual_color};"
+			url_set_motor_pos += f"m{id + 1}:{v}:{send_dt};"
 
 
 		hg_m["acc"] = lerp(0.1, hg_m["acc"], clamp((hg_m["v"] - v) / (hg.time_to_sec_f(dt)**2), -9999, 9999))
@@ -241,16 +337,18 @@ def get_v_from_dancing(id_robot):
 
 		hg_m["n"].GetTransform().SetRot(rot)
 
-	# send value
-	timer_requests_not_overload += hg.time_to_sec_f(dt)
-	if timer_requests_not_overload > send_dt:
-		timer_requests_not_overload = 0
+	# check if we are getting value from the real root
+	if not compliance_mode:
+		# send value
+		timer_requests_not_overload += hg.time_to_sec_f(dt)
+		if timer_requests_not_overload > send_dt:
+			timer_requests_not_overload = 0
 
-		if url != "":
-			try:
-				requests.get(url_set_motor_pos[:-1])
-			except:
-				print("Robot not connected "+url)
+			if url != "":
+				try:
+					requests.get(url_set_motor_pos[:-1])
+				except:
+					print("Robot not connected "+url)
 
 	# led ping pong
 	led_current_motor_timer_ping_pong += hg.time_to_sec_f(dt)
@@ -391,8 +489,11 @@ def get_v_from_dancing(id_robot):
 					mat, hg.Vec3(0, 0, 0), hg.DTHA_Left, hg.DTVA_Top,
 					[font_color], [], text_render_state)
 
+	compliance_mode = toggle_button("Compliance Mode ON" if compliance_mode else "Compliance Mode OFF", compliance_mode, 100, res_y - 80)
+
 	view_id += 1
 
+
 	current_frame = hg.Frame()
 	hg.UpdateWindow(win)