From d8558b4392977ad55b1339eccd7bb024fc34573a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Janu=C5=A1=20Likozar?= Date: Sun, 19 Sep 2021 13:13:23 +0200 Subject: [PATCH] added lean to move files --- lean-to-move/.gitignore | 3 + lean-to-move/CMakeLists.txt | 11 + .../bindings/hiplocomotion_actions.json | 83 ++++++ .../hiplocomotion_bindings_empty.json | 5 + .../hiplocomotion_bindings_generic.json | 20 ++ .../hiplocomotion_bindings_knuckles.json | 20 ++ ...iplocomotion_bindings_vive_controller.json | 20 ++ .../hiplocomotion_bindings_vive_tracker.json | 15 + lean-to-move/hiplocomotion.cpp | 280 ++++++++++++++++++ lean-to-move/hiplocomotion.h | 15 + lean-to-move/hiplocomotion.py | 208 +++++++++++++ lean-to-move/hiplocomotion.spec | 38 +++ 12 files changed, 718 insertions(+) create mode 100644 lean-to-move/.gitignore create mode 100644 lean-to-move/CMakeLists.txt create mode 100644 lean-to-move/bindings/hiplocomotion_actions.json create mode 100644 lean-to-move/bindings/hiplocomotion_bindings_empty.json create mode 100644 lean-to-move/bindings/hiplocomotion_bindings_generic.json create mode 100644 lean-to-move/bindings/hiplocomotion_bindings_knuckles.json create mode 100644 lean-to-move/bindings/hiplocomotion_bindings_vive_controller.json create mode 100644 lean-to-move/bindings/hiplocomotion_bindings_vive_tracker.json create mode 100644 lean-to-move/hiplocomotion.cpp create mode 100644 lean-to-move/hiplocomotion.h create mode 100644 lean-to-move/hiplocomotion.py create mode 100644 lean-to-move/hiplocomotion.spec diff --git a/lean-to-move/.gitignore b/lean-to-move/.gitignore new file mode 100644 index 0000000..f0779f3 --- /dev/null +++ b/lean-to-move/.gitignore @@ -0,0 +1,3 @@ +build/* +dist/* +release/* \ No newline at end of file diff --git a/lean-to-move/CMakeLists.txt b/lean-to-move/CMakeLists.txt new file mode 100644 index 0000000..ae8913b --- /dev/null +++ b/lean-to-move/CMakeLists.txt @@ -0,0 +1,11 @@ +# CMakeList.txt : CMake project for HipLocomotion, include source and define +# project specific logic here. +# +cmake_minimum_required (VERSION 3.8) + + +# Add source to this project's executable. +add_executable (hiplocomotion "hiplocomotion.cpp" "hiplocomotion.h") + +target_include_directories("hiplocomotion" PUBLIC "${OPENVR_INCLUDE_DIR}") +target_link_libraries("hiplocomotion" PUBLIC "${OPENVR_LIB}") \ No newline at end of file diff --git a/lean-to-move/bindings/hiplocomotion_actions.json b/lean-to-move/bindings/hiplocomotion_actions.json new file mode 100644 index 0000000..6dd85bb --- /dev/null +++ b/lean-to-move/bindings/hiplocomotion_actions.json @@ -0,0 +1,83 @@ +{ + "default_bindings": [ + { + "controller_type": "vive_controller", + "binding_url": "hiplocomotion_bindings_vive_controller.json" + }, + { + "controller_type": "vive_tracker_waist", + "binding_url": "hiplocomotion_bindings_vive_tracker.json" + }, + { + "controller_type": "generic", + "binding_url": "hiplocomotion_bindings_generic.json" + }, + { + "controller_type": "vive_tracker_left_foot", + "binding_url": "hiplocomotion_bindings_empty.json" + }, + { + "controller_type": "vive_tracker_right_foot", + "binding_url": "hiplocomotion_bindings_empty.json" + }, + { + "controller_type": "vive_tracker_left_shoulder", + "binding_url": "hiplocomotion_bindings_empty.json" + }, + { + "controller_type": "vive_tracker_right_shoulder", + "binding_url": "hiplocomotion_bindings_empty.json" + }, + { + "controller_type": "vive_tracker_left_elbow", + "binding_url": "hiplocomotion_bindings_empty.json" + }, + { + "controller_type": "vive_tracker_right_elbow", + "binding_url": "hiplocomotion_bindings_empty.json" + }, + { + "controller_type": "vive_tracker_left_knee", + "binding_url": "hiplocomotion_bindings_empty.json" + }, + { + "controller_type": "vive_tracker_right_knee", + "binding_url": "hiplocomotion_bindings_empty.json" + }, + { + "controller_type": "vive_tracker_chest", + "binding_url": "hiplocomotion_bindings_empty.json" + }, + { + "controller_type": "vive_tracker_camera", + "binding_url": "hiplocomotion_bindings_empty.json" + }, + { + "controller_type": "vive_tracker_keyboard", + "binding_url": "hiplocomotion_bindings_empty.json" + } + ], + "actions": [ + { + "name": "/actions/demo/in/AnalogInput", + "type": "vector2" + }, + { + "name": "/actions/demo/in/Tracker", + "type": "pose" + } + ], +"action_sets": [ + { + "name": "/actions/demo", + "usage": "leftright" + } + ], + "localization" : [ + { + "language_tag": "en_US", + "/actions/demo/in/AnalogInput" : "Analog Input", + "/actions/demo/in/Tracker" : "Tracker Pose" + } + ] +} \ No newline at end of file diff --git a/lean-to-move/bindings/hiplocomotion_bindings_empty.json b/lean-to-move/bindings/hiplocomotion_bindings_empty.json new file mode 100644 index 0000000..e31849f --- /dev/null +++ b/lean-to-move/bindings/hiplocomotion_bindings_empty.json @@ -0,0 +1,5 @@ +{ + "controller_type" : "vive_tracker", + "description" : "Empty bindings for non hip trackers", + "name" : "Empty binding" +} \ No newline at end of file diff --git a/lean-to-move/bindings/hiplocomotion_bindings_generic.json b/lean-to-move/bindings/hiplocomotion_bindings_generic.json new file mode 100644 index 0000000..4889b4b --- /dev/null +++ b/lean-to-move/bindings/hiplocomotion_bindings_generic.json @@ -0,0 +1,20 @@ +{ + "bindings" : { + "/actions/demo" : { + "sources" : [ + { + "inputs" : { + "position" : { + "output" : "/actions/demo/in/analoginput" + } + }, + "mode" : "joystick", + "path" : "/user/hand/left/input/joystick" + } + ] + } + }, + "controller_type" : "generic", + "description" : "Bindings for the hip locomotion for a generic controller", + "name" : "Hip locomotion bindings for a generic controller" +} \ No newline at end of file diff --git a/lean-to-move/bindings/hiplocomotion_bindings_knuckles.json b/lean-to-move/bindings/hiplocomotion_bindings_knuckles.json new file mode 100644 index 0000000..875e85a --- /dev/null +++ b/lean-to-move/bindings/hiplocomotion_bindings_knuckles.json @@ -0,0 +1,20 @@ +{ + "bindings" : { + "/actions/demo" : { + "sources" : [ + { + "inputs" : { + "position" : { + "output" : "/actions/demo/in/analoginput" + } + }, + "mode" : "joystick", + "path" : "/user/hand/left/input/thumbstick" + } + ] + } + }, + "controller_type" : "knuckles", + "description" : "Bindings for the hip locomotion for a knuckles controller", + "name" : "Hip locomotion bindings for a knuckles controller" +} \ No newline at end of file diff --git a/lean-to-move/bindings/hiplocomotion_bindings_vive_controller.json b/lean-to-move/bindings/hiplocomotion_bindings_vive_controller.json new file mode 100644 index 0000000..52da785 --- /dev/null +++ b/lean-to-move/bindings/hiplocomotion_bindings_vive_controller.json @@ -0,0 +1,20 @@ +{ + "bindings" : { + "/actions/demo" : { + "sources" : [ + { + "inputs" : { + "position" : { + "output" : "/actions/demo/in/analoginput" + } + }, + "mode" : "trackpad", + "path" : "/user/hand/right/input/trackpad" + } + ] + } + }, + "controller_type" : "vive_controller", + "description" : "Bindings for hip locomotion for the Vive controller", + "name" : "Hip locomotion bindings for Vive Controller" +} \ No newline at end of file diff --git a/lean-to-move/bindings/hiplocomotion_bindings_vive_tracker.json b/lean-to-move/bindings/hiplocomotion_bindings_vive_tracker.json new file mode 100644 index 0000000..aa35d6d --- /dev/null +++ b/lean-to-move/bindings/hiplocomotion_bindings_vive_tracker.json @@ -0,0 +1,15 @@ +{ + "bindings" : { + "/actions/demo" : { + "poses" : [ + { + "output" : "/actions/demo/in/Tracker", + "path" : "/user/waist/pose/raw" + } + ] + } + }, + "controller_type" : "vive_tracker", + "description" : "Bindings for the hip locomotion for a tracker", + "name" : "Hip locomotion bindings for a tracker" +} \ No newline at end of file diff --git a/lean-to-move/hiplocomotion.cpp b/lean-to-move/hiplocomotion.cpp new file mode 100644 index 0000000..d532f16 --- /dev/null +++ b/lean-to-move/hiplocomotion.cpp @@ -0,0 +1,280 @@ +#include "hiplocomotion.h" + +const int BUFSIZE = 1024; + +TCHAR chReadBuf[BUFSIZE]; +BOOL fSuccess; +DWORD cbRead; +LPTSTR lpszPipename = TEXT("\\\\.\\pipe\\ApriltagPipeIn"); + +int trackernum = 4; + +void check_error(int line, vr::EVRInitError error) { if (error != 0) printf("%d: error %s\n", line, VR_GetVRInitErrorAsSymbol(error)); } + +int main() +{ + vr::EVRInitError error; + VR_Init(&error, vr::VRApplication_Overlay); + check_error(__LINE__, error); + + std::istringstream ret; + std::string word; + + ret = Send(TEXT("addhipmove")); + ret >> word; + if (word != "added") + { + std::cout << "Wrong message received!" << std::endl; + } + /* + vr::VROverlayHandle_t handle; + vr::VROverlay()->CreateOverlay("image", "image", &handle); /* key has to be unique, name doesn't matter + vr::VROverlay()->SetOverlayFromFile(handle, "nothing.jpg"); + vr::VROverlay()->SetOverlayWidthInMeters(handle, 3); + vr::VROverlay()->ShowOverlay(handle); + + vr::HmdMatrix34_t transform = { + 1.0f, 0.0f, 0.0f, 0.0f, + 0.0f, 1.0f, 0.0f, 1.0f, + 0.0f, 0.0f, 1.0f, -2.0f + }; + vr::VROverlay()->SetOverlayTransformAbsolute(handle, vr::TrackingUniverseStanding, &transform); + + */ + + vr::VRActionHandle_t m_actionAnalongInput = vr::k_ulInvalidActionHandle; + vr::VRActionHandle_t m_actionsetDemo = vr::k_ulInvalidActionHandle; + vr::VRActionHandle_t m_actionPose = vr::k_ulInvalidActionHandle; + + DWORD retval = 0; + BOOL success; + TCHAR buffer[BUFSIZE] = TEXT(""); + TCHAR buf[BUFSIZE] = TEXT(""); + TCHAR** lppPart = { NULL }; + + retval = GetFullPathName("hiplocomotion_actions.json", + BUFSIZE, + buffer, + lppPart); + + if (retval == 0) + { + // Handle an error condition. + printf("GetFullPathName failed (%d)\n", GetLastError()); + return -1; + } + else + { + std::cout << "The full path name is: " << buffer << std::endl; + if (lppPart != NULL && *lppPart != 0) + { + std::cout << "The final component in the path name is: " << *lppPart <SetActionManifestPath(buffer); + + vr::VRInput()->GetActionHandle("/actions/demo/in/AnalogInput", &m_actionAnalongInput); + vr::VRInput()->GetActionHandle("/actions/demo/in/Tracker", &m_actionPose); + + vr::VRInput()->GetActionSetHandle("/actions/demo", &m_actionsetDemo); + + vr::TrackedDevicePose_t pTrackedDevicePose[10]; + + vr::VRSystem()->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseStanding,0, pTrackedDevicePose,10); + + float controllerRotation = 0; + float hmdPosition[2] = { 0,0 }; + float hmdRotation = 0; + + float offsetHmd = 0; + float offsetHip = 0; + + float centerHmd[2] = { 0,0 }; + + bool recalibrate = true; + + std::cout << "Hip locomotion started!" << std::endl; + + Sleep(1000); + + int counter = 0; + + float neckOffset[3] = { 0,-0.1,0.1 }; + + while (true) { + + vr::VRSystem()->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseStanding, 0, pTrackedDevicePose, 10); + + vr::VRActiveActionSet_t actionSet = { 0 }; + actionSet.ulActionSet = m_actionsetDemo; + vr::VRInput()->UpdateActionState(&actionSet, sizeof(actionSet), 1); + + + //std::cout << pTrackedDevicePose[0].mDeviceToAbsoluteTracking.m[0][3] << " " << pTrackedDevicePose[0].mDeviceToAbsoluteTracking.m[1][3] << " " << pTrackedDevicePose[0].mDeviceToAbsoluteTracking.m[2][3] << std::endl; + hmdRotation = atan2(pTrackedDevicePose[0].mDeviceToAbsoluteTracking.m[0][2], pTrackedDevicePose[0].mDeviceToAbsoluteTracking.m[2][2]); + + + + hmdPosition[0] = pTrackedDevicePose[0].mDeviceToAbsoluteTracking.m[0][3] + + neckOffset[0] * pTrackedDevicePose[0].mDeviceToAbsoluteTracking.m[0][0] + + neckOffset[1] * pTrackedDevicePose[0].mDeviceToAbsoluteTracking.m[0][1] + + neckOffset[2] * pTrackedDevicePose[0].mDeviceToAbsoluteTracking.m[0][2]; + hmdPosition[1] = pTrackedDevicePose[0].mDeviceToAbsoluteTracking.m[2][3] + + neckOffset[0] * pTrackedDevicePose[0].mDeviceToAbsoluteTracking.m[2][0] + + neckOffset[1] * pTrackedDevicePose[0].mDeviceToAbsoluteTracking.m[2][1] + + neckOffset[2] * pTrackedDevicePose[0].mDeviceToAbsoluteTracking.m[2][2]; + + + vr::InputAnalogActionData_t analogData; + if (vr::VRInput()->GetAnalogActionData(m_actionAnalongInput, &analogData, sizeof(analogData), vr::k_ulInvalidInputValueHandle) == vr::VRInputError_None && analogData.bActive) + { + //std::cout << "x: " << analogData.x << " y: " << analogData.y << std::endl; + //SendMove(analogData.x, analogData.y); + } + else + { + Sleep(100); + recalibrate = true; + SendMove(0, 0, 0, 0, 0, 0); + continue; + } + + vr::InputPoseActionData_t poseData; + + //std::cout << "error:" <GetPoseActionDataForNextFrame(m_actionPose, vr::TrackingUniverseStanding, &poseData, sizeof(poseData), vr::k_ulInvalidInputValueHandle) << std::endl; + if (vr::VRInput()->GetPoseActionDataForNextFrame(m_actionPose, vr::TrackingUniverseStanding, &poseData, sizeof(poseData), vr::k_ulInvalidInputValueHandle) == vr::VRInputError_None) + { + //std::cout << "controller pose: " << poseData.pose.mDeviceToAbsoluteTracking.m[0][3] << " " << poseData.pose.mDeviceToAbsoluteTracking.m[1][3] << " " << poseData.pose.mDeviceToAbsoluteTracking.m[2][3] << std::endl; + controllerRotation = atan2(poseData.pose.mDeviceToAbsoluteTracking.m[0][2], poseData.pose.mDeviceToAbsoluteTracking.m[2][2]); + + //hmdPosition[0] = poseData.pose.mDeviceToAbsoluteTracking.m[0][3]; + //hmdPosition[1] = poseData.pose.mDeviceToAbsoluteTracking.m[2][3]; + + } + + if (recalibrate) + { + offsetHip = controllerRotation; + offsetHmd = hmdRotation; + centerHmd[0] = hmdPosition[0]; + centerHmd[1] = hmdPosition[1]; + recalibrate = false; + } + + hmdPosition[0] -= centerHmd[0]; + hmdPosition[1] -= centerHmd[1]; + + float magnitude = sqrt(hmdPosition[0] * hmdPosition[0] + hmdPosition[1] * hmdPosition[1]); + float angle = atan2(hmdPosition[0], hmdPosition[1]); + + float newDataX = 0; + float newDataY = 0; + + //magnitude = min(magnitude, 0.15); + + if (magnitude > 0.1) + { + newDataX = sin(angle - hmdRotation) * (magnitude-0.1)/0.1; + newDataY = cos(angle - hmdRotation) * (magnitude-0.1)/0.1; + } + + + float angleRot = controllerRotation - offsetHip; + //float angleRot = hmdRotation - offsetHmd; + if (angleRot > 3.14) + angleRot -= 6.28; + if (angleRot < -3.14) + angleRot += 6.28; + + float newDataRx = 0; + + if (abs(angleRot) > 0.3) + { + newDataRx = (abs(angleRot) - 0.3) / 0.2; + newDataRx /= -abs(angleRot) / angleRot; + } + + std::cout << "Angle data:" << angleRot << ", " << newDataRx << std::endl; + + std::cout << "Received data: " << analogData.x << "," << analogData.y << " Calculated data: " << newDataX << "," << newDataY << std::endl; + + SendMove(newDataX, newDataY, newDataRx, 0, 0, 0); + + Sleep(2); + + } + return 0; +} + +bool GetDigitalActionState(vr::VRActionHandle_t action, vr::VRInputValueHandle_t* pDevicePath) +{ + vr::InputDigitalActionData_t actionData; + vr::VRInput()->GetDigitalActionData(action, &actionData, sizeof(actionData), vr::k_ulInvalidInputValueHandle); + if (pDevicePath) + { + *pDevicePath = vr::k_ulInvalidInputValueHandle; + if (actionData.bActive) + { + vr::InputOriginInfo_t originInfo; + if (vr::VRInputError_None == vr::VRInput()->GetOriginTrackedDeviceInfo(actionData.activeOrigin, &originInfo, sizeof(originInfo))) + { + *pDevicePath = originInfo.devicePath; + } + } + } + return actionData.bActive && actionData.bState; +} + +std::istringstream Send(LPTSTR lpszWrite) +{ + fSuccess = CallNamedPipe( + lpszPipename, // pipe name + lpszWrite, // message to server + (lstrlen(lpszWrite) + 1) * sizeof(TCHAR), // message length + chReadBuf, // buffer to receive reply + BUFSIZE * sizeof(TCHAR), // size of read buffer + &cbRead, // number of bytes read + 2000); // waits for 2 seconds + + if (fSuccess || GetLastError() == ERROR_MORE_DATA) + { + std::cout << chReadBuf << std::endl; + chReadBuf[cbRead] = '\0'; //add terminating zero + //convert our buffer to string + std::string rec = chReadBuf; + std::istringstream iss(rec); + // The pipe is closed; no more data can be read. + + if (!fSuccess) + { + printf("\nExtra data in message was lost\n"); + } + return iss; + } + else + { + std::cout << GetLastError() << " :(" << std::endl; + std::string rec = " senderror"; + std::istringstream iss(rec); + return iss; + } +} + +std::istringstream SendMove(double x, double y, double rx, double ry, double a, double b) +{ + std::string s; + s = " hipmoveinput " + std::to_string(x) + + " " + std::to_string(y) + + " " + std::to_string(rx) + + " " + std::to_string(ry) + + " " + std::to_string(a) + + " " + std::to_string(b) + "\n"; + + //send the string to our driver + + LPTSTR sendstring = (LPTSTR)s.c_str(); + + return Send(sendstring); +} \ No newline at end of file diff --git a/lean-to-move/hiplocomotion.h b/lean-to-move/hiplocomotion.h new file mode 100644 index 0000000..ffeab70 --- /dev/null +++ b/lean-to-move/hiplocomotion.h @@ -0,0 +1,15 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +std::istringstream SendMove(double x, double y, double rx, double ry, double a, double b); +std::istringstream Send(LPTSTR lpszWrite); +bool GetDigitalActionState(vr::VRActionHandle_t action, vr::VRInputValueHandle_t* pDevicePath = nullptr); diff --git a/lean-to-move/hiplocomotion.py b/lean-to-move/hiplocomotion.py new file mode 100644 index 0000000..3fe4af8 --- /dev/null +++ b/lean-to-move/hiplocomotion.py @@ -0,0 +1,208 @@ +print("Importing libraries....") + +import os +import sys + +sys.path.append(os.getcwd()) #embedable python doesnt find local modules without this line + +import time +import threading +import numpy as np +from scipy.spatial.transform import Rotation as R +import openvr +import tkinter as tk + +moveThr = 0.1 +moveMax = 0.2 + +rotThr = 0.3 +rotMax = 0.5 + +def gui_thread(): + + global moveThr, moveMax, rotThr, rotMax + + def save(): + print("Saved parameters") + moveThr = mtVar.get() + moveMax = mmVar.get() + rotThr = rtVar.get() + rotMax = rmVar.get() + + print("new thread started") + window = tk.Tk() + + mtVar = tk.DoubleVar(value = moveThr) + tk.Label(text="Move threshold", width = 50).pack() + mt = tk.Scale(length=300,from_=0,to=1, digits=3,variable=mtVar,resolution=0.01, orient=tk.HORIZONTAL) + mt.pack() + + mmVar = tk.DoubleVar(value = moveMax) + tk.Label(text="Move maximum", width = 50).pack() + mm = tk.Scale(length=300,from_=0,to=1, digits=3,variable=mmVar,resolution=0.01, orient=tk.HORIZONTAL) + mm.pack() + + rtVar = tk.DoubleVar(value = rotThr) + tk.Label(text="Rotation threshold", width = 50).pack() + rt = tk.Scale(length=300,from_=0,to=1, digits=3,variable=rtVar,resolution=0.01, orient=tk.HORIZONTAL) + rt.pack() + + rmVar = tk.DoubleVar(value = rotMax) + tk.Label(text="Rotation maximum", width = 50).pack() + rm = tk.Scale(length=300,from_=0,to=1, digits=3,variable=rmVar,resolution=0.01, orient=tk.HORIZONTAL) + rm.pack() + + tk.Button(text='Save', command=save).pack() + + window.mainloop() + +def sendToSteamVR(text): + #Function to send a string to my steamvr driver through a named pipe. + #open pipe -> send string -> read string -> close pipe + #sometimes, something along that pipeline fails for no reason, which is why the try catch is needed. + #returns an array containing the values returned by the driver. + try: + pipe = open(r'\\.\pipe\ApriltagPipeIn', 'rb+', buffering=0) + some_data = str.encode(text) + pipe.write(some_data) + resp = pipe.read(1024) + except: + return ["error"] + string = resp.decode("utf-8") + array = string.split(" ") + pipe.close() + + return array + +def SendMove(x, y, rx, ry, a, b): + return sendToSteamVR(" hipmoveinput " + + str(x) + " " + + str(y) + " " + + str(rx) + " " + + str(ry) + " " + + str(a) + " " + + str(b)) + +def convert_steam_vr_matrix(pose): + return np.array(( + (pose[0][0], pose[1][0], pose[2][0], 0.0), + (pose[0][1], pose[1][1], pose[2][1], 0.0), + (pose[0][2], pose[1][2], pose[2][2], 0.0), + (pose[0][3], pose[1][3], pose[2][3], 1.0), + ), dtype=np.float32) + +print("Connecting to SteamVR") + +gui = threading.Thread(target=gui_thread) +gui.start() + +time.sleep(1) + +handle = openvr.init(openvr.VRApplication_Overlay) + +sendToSteamVR("addhipmove") + +m_actionAnalongInput = openvr.k_ulInvalidActionHandle; +m_actionsetDemo = openvr.k_ulInvalidActionHandle; +m_actionPose = openvr.k_ulInvalidActionHandle; + +openvr.VRInput().setActionManifestPath(os.getcwd()+"/bindings/hiplocomotion_actions.json") + +m_actionAnalongInput = openvr.VRInput().getActionHandle("/actions/demo/in/AnalogInput") +m_actionPose = openvr.VRInput().getActionHandle("/actions/demo/in/Tracker") +m_actionsetDemo = openvr.VRInput().getActionSetHandle('/actions/demo') + +trackerRotation = 0 +hmdPosition = [ 0,0 ] +hmdRotation = 0 + +offsetHmd = 0 +offsetHip = 0 + +centerHmd = [ 0,0 ] + +recalibrate = True + +print("Hip locomotion started!") + +time.sleep(1) + +counter = 0 + +neckOffset = np.array([ 0,-0.1,0.1,1 ]) + +while True: + hmd_pose = convert_steam_vr_matrix(handle.getDeviceToAbsoluteTrackingPose(openvr.TrackingUniverseStanding,0,[])[0].mDeviceToAbsoluteTracking) + #print(hmd_pose) + hmdRotation = np.arctan2(hmd_pose[0][2],hmd_pose[2][2]) + + hmd_pos = np.dot(neckOffset,hmd_pose) + + #print(hmd_pos) + hmdPosition = [hmd_pos[0],hmd_pos[2]] + + action_sets = (openvr.VRActiveActionSet_t * 1)() + action_set = action_sets[0] + action_set.ulActionSet = m_actionsetDemo + openvr.VRInput().updateActionState(action_sets) + + analog_data = openvr.VRInput().getAnalogActionData(m_actionAnalongInput, openvr.k_ulInvalidInputValueHandle) + if not analog_data.bActive: + time.sleep(0.1) + recalibrate = True + SendMove(0,0,0,0,0,0) + continue + + pose_data = openvr.VRInput().getPoseActionDataForNextFrame( + m_actionPose, + openvr.TrackingUniverseStanding, + openvr.k_ulInvalidInputValueHandle, + ) + """if not pose_data.bActive: + print(pose_data.bActive) + time.sleep(0.1) + continue""" + + tracker_pose = convert_steam_vr_matrix(pose_data.pose.mDeviceToAbsoluteTracking) + trackerRotation = np.arctan2(tracker_pose[0][2],tracker_pose[2][2]) + + if recalibrate: + offsetHip = trackerRotation + offsetHmd = hmdRotation + centerHmd = [hmdPosition[0],hmdPosition[1]] + recalibrate = False + + hmdPosition[0] -= centerHmd[0] + hmdPosition[1] -= centerHmd[1] + + magnitude = np.sqrt(hmdPosition[0]**2 + hmdPosition[1]**2) + angle = np.arctan2(hmdPosition[0],hmdPosition[1]) + + newDataX = 0 + newDataY = 0 + + if(magnitude > moveThr): + newDataX = np.sin(angle+hmdRotation) * (magnitude-moveThr)/(moveMax-moveThr) + newDataY = np.cos(angle+hmdRotation) * (magnitude-moveThr)/(moveMax-moveThr) + + angleRot = trackerRotation - offsetHip + + if angleRot > 3.14: + angleRot -= 6.28 + if angleRot < -3.14: + angleRot += 6.28 + + newDataRx = 0 + + if abs(angleRot) > rotThr: + newDataRx = (abs(angleRot)-rotThr)/(rotMax-rotThr) + newDataRx /= -abs(angleRot)/angleRot + + print("Sending data: " + str(newDataX) + ", " + str(newDataY) + ", rotation: " + str(newDataRx)) + + print(SendMove(newDataX,newDataY,newDataRx, 0, 0, 0)) + + time.sleep(0.02) + + if not gui.is_alive(): + break diff --git a/lean-to-move/hiplocomotion.spec b/lean-to-move/hiplocomotion.spec new file mode 100644 index 0000000..78044ab --- /dev/null +++ b/lean-to-move/hiplocomotion.spec @@ -0,0 +1,38 @@ +# -*- mode: python ; coding: utf-8 -*- + + +block_cipher = None + + +a = Analysis(['hiplocomotion.py'], + pathex=['G:\\vr test stuff\\Tracker-Driver\\hip_locomotion'], + binaries=[('G:\\Python\\Python396\\Lib\\site-packages\\openvr\\libopenvr_api_64.dll','.\\openvr')], + datas=[], + hiddenimports=[], + hookspath=[], + runtime_hooks=[], + excludes=[], + win_no_prefer_redirects=False, + win_private_assemblies=False, + cipher=block_cipher, + noarchive=False) +pyz = PYZ(a.pure, a.zipped_data, + cipher=block_cipher) +exe = EXE(pyz, + a.scripts, + [], + exclude_binaries=True, + name='hiplocomotion', + debug=False, + bootloader_ignore_signals=False, + strip=False, + upx=True, + console=True ) +coll = COLLECT(exe, + a.binaries, + a.zipfiles, + a.datas, + strip=False, + upx=True, + upx_exclude=[], + name='hiplocomotion')