-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy path048_server.py
180 lines (151 loc) · 6.77 KB
/
048_server.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
import zmq
import sys
sys.path.append('utils')
import rebocap_ws_sdk
import numpy as np
from scipy.spatial.transform import Rotation as R
from time import sleep
import socket
import re
def get_link(sdk):
tran, pose24, static_index, tp = sdk.get_last_msg()
pose24_np = np.array(pose24)#.tobytes()
X_WorldRebocap = np.eye(4)
X_WorldRebocap[:3, :3] = R.from_euler("x",[90.0] , degrees=True).as_matrix()
X_RebocapLink9 = np.eye(4)
X_RebocapLink9[:3, :3] = R.from_quat(pose24_np[9]).as_matrix()
X_RebocapLink9[1, 3] = 1.25 # chest2ground height
X_RebocapLink16 = np.eye(4)
X_Link9Link16 = np.eye(4)
X_Link9Link16[:3, :3] = R.from_quat(pose24_np[16]).as_matrix()
X_Link9Link16[0, 3] = 0.17 # shoulder width
X_Link9Link16[1, 3] = 0.242 # shoulder height
X_RebocapLink17 = np.eye(4)
X_Link9Link17 = np.eye(4)
X_Link9Link17[:3, :3] = R.from_quat(pose24_np[17]).as_matrix()
X_Link9Link17[0, 3] = -0.17 # shoulder width
X_Link9Link17[1, 3] = 0.242 # shoulder height
X_RebocapLink18 = np.eye(4)
X_Link16Link18 = np.eye(4)
X_Link16Link18[:3, :3] = R.from_quat(pose24_np[18]).as_matrix()
X_Link16Link18[0, 3] = 0.257 # upper arm length
X_RebocapLink19 = np.eye(4)
X_Link17Link19 = np.eye(4)
X_Link17Link19[:3, :3] = R.from_quat(pose24_np[19]).as_matrix()
X_Link17Link19[0, 3] = -0.257 # upper arm length
X_RebocapLink20 = np.eye(4)
X_Link18Link20 = np.eye(4)
X_Link18Link20[:3, :3] = R.from_quat(pose24_np[20]).as_matrix()
X_Link18Link20[0, 3] = 0.266 # lower arm length
X_RebocapLink21 = np.eye(4)
X_Link19Link21 = np.eye(4)
X_Link19Link21[:3, :3] = R.from_quat(pose24_np[21]).as_matrix()
X_Link19Link21[0, 3] = -0.266 # lower arm length
X_RebocapLink22 = np.eye(4)
X_Link20Link22 = np.eye(4)
X_Link20Link22[:3, :3] = R.from_quat(pose24_np[22]).as_matrix()
X_Link20Link22[0, 3] = 0.04 # hand length
X_RebocapLink23 = np.eye(4)
X_Link21Link23 = np.eye(4)
X_Link21Link23[:3, :3] = R.from_quat(pose24_np[23]).as_matrix()
X_Link21Link23[0, 3] = -0.04 # hand length
X_RebocapLink16 = X_RebocapLink9 @ X_Link9Link16
X_RebocapLink17 = X_RebocapLink9 @ X_Link9Link17
X_RebocapLink18 = X_RebocapLink16 @ X_Link16Link18
X_RebocapLink19 = X_RebocapLink17 @ X_Link17Link19
X_RebocapLink20 = X_RebocapLink18 @ X_Link18Link20
X_RebocapLink21 = X_RebocapLink19 @ X_Link19Link21
X_RebocapLink22 = X_RebocapLink20 @ X_Link20Link22
X_RebocapLink23 = X_RebocapLink21 @ X_Link21Link23
X_WorldLink9 = X_WorldRebocap @ X_RebocapLink9
X_WorldLink16 = X_WorldRebocap @ X_RebocapLink16
X_WorldLink17 = X_WorldRebocap @ X_RebocapLink17
X_WorldLink18 = X_WorldRebocap @ X_RebocapLink18
X_WorldLink19 = X_WorldRebocap @ X_RebocapLink19
X_WorldLink20 = X_WorldRebocap @ X_RebocapLink20
X_WorldLink21 = X_WorldRebocap @ X_RebocapLink21
X_WorldLink22 = X_WorldRebocap @ X_RebocapLink22
X_WorldLink23 = X_WorldRebocap @ X_RebocapLink23
return X_WorldLink22, X_WorldLink23
counter = 0
def main():
def print_debug_msg(self: rebocap_ws_sdk.RebocapWsSdk, trans, pose24, static_inlocaldex, ts):
global counter
is_left_on_floor = 0 <= static_index <= 5
is_right_on_floor = 6 <= static_index <= 11
no_foot_on_ground = static_index < 0
if counter % 60 == 0:
print(f'timestamp:{ts}'
f'current coordinate_type: {self.coordinate_type.name}'
f'root position:{trans} left_on_floor:{is_left_on_floor} right_on_floor:{is_right_on_floor}')
for i in range(24):
print(f'bone:{rebocap_ws_sdk.REBOCAP_JOINT_NAMES[i]} quaternion w,x,y,z is:{pose24[i]}')
print('\n\n\n\n', flush=True)
counter += 1
# 姿态数据回调
def pose_msg_callback(self: rebocap_ws_sdk.RebocapWsSdk, tran: list, pose24: list, static_index: int, ts: float):
print_debug_msg(self, tran, pose24, static_index, ts)
pass
# 异常断开，这里处理重连或报错
def exception_close_callback(self: rebocap_ws_sdk.RebocapWsSdk):
print("exception_close_callback")
# server (glove)
host, port = '0.0.0.0', 5559
glove_server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
glove_server_socket.bind((host, port))
glove_server_socket.listen(1)
print(f"Server listening on {host}:{port}")
glove_server_socket.settimeout(10.0)
glove_client_socket, client_address = glove_server_socket.accept()
print(f"Connection from {client_address} established.")
# rebocap initialize
# 初始化sdk 这里可以选择控件坐标系， 控件坐标系目前已经测试过的有： 1. UE 2. Unity 3. Blender
# 选择输出角度是否是 global 角度，默认是 local 角度【简单解释，global 角度不受父节点影响 local角度受父节点影响， local角度逐级相乘就是 global 角度
sdk = rebocap_ws_sdk.RebocapWsSdk(coordinate_type=rebocap_ws_sdk.CoordinateType.DefaultCoordinate, use_global_rotation=False)
# 设置姿态回调
sdk.set_pose_msg_callback(pose_msg_callback)
# 设置异常断开回调
sdk.set_exception_close_callback(exception_close_callback)
# 开始连接
open_ret = sdk.open(7690)
# 检查连接状态
if open_ret == 0:
print("连接成功")
else:
print("连接失败", open_ret)
if open_ret == 1:
print("连接状态错误")
elif open_ret == 2:
print("连接失败")
elif open_ret == 3:
print("认证失败")
else:
print("未知错误", open_ret)
exit(1)
# # server (motion)
context = zmq.Context()
# # # talk to client linux machine
motion_socket = context.socket(zmq.REP) # we r server
motion_socket.bind("tcp://*:5555")
while True:
while True:
data = ""
chunk = glove_client_socket.recv(65536)
data += chunk.decode("utf-8")
right_hand_matches = re.findall(r"R\d+:\s(-?\d+\.\d+)", data)
right_hand_data = [float(value) for value in right_hand_matches[-28:]]
if len(right_hand_data) == 28:
break
right_hand_data = np.array(right_hand_data).reshape((-1, 4))
X_WorldLink22, X_WorldLink23 = get_link(sdk)
X_WorldLink22_23_rhanddata = np.concatenate([X_WorldLink22, X_WorldLink23, right_hand_data], axis=0)
# print(X_WorldLink22)
# print(X_WorldLink23)
# print(right_hand_data)
# print(X_WorldLink22_23_rhanddata.shape)
# print(X_WorldLink22_23_rhanddata)
message_send = X_WorldLink22_23_rhanddata.tobytes()
message_recv = motion_socket.recv()
motion_socket.send(message_send)
if __name__ == "__main__":
main()