joycon相关
joycon的传感器本身分辨率很低，没有磁力计，仅imu和加速度计，因此仅能依赖积分来获得对朝向的估计，而且随着时间推移会有累积误差，无法获得绝对朝向和位置。
关于joycon-R带的IR摄像头，我也不认为它能做太多的事情。将其固定在特定位置，（如纸壳钢琴中检测遮挡，我觉得这就是极限了）
device
https://betterjoy.net/connect-joy-cons-to-pc/
按住SL和SR中间的按钮5s开始配对。而非其他LR, LT等按键
可能用到的姿态和依赖库
https://ahrs.readthedocs.io/en/latest/filters/madgwick.html
hidapi
1
2
3
import hid
raise ImportError(error)
ImportError: Unable to load any of the following libraries:libhidapi-hidraw.so libhidapi-hidraw.so.0 libhidapi-libusb.so libhidapi-libusb.so.0 libhidapi-iohidmanager.so libhidapi-iohidmanager.so.0 libhidapi.dylib hidapi.dll libhidapi-0.dll
https://github.com/libusb/hidapi/releases
将解压得到的dll放到与脚本同一目录，然后在python代码中添加:
1
2
dll_path = "./hidapi.dll"
ctypes.CDLL(dll_path)
即可接受识别joycon返回数据。
完整的导入代码，接受一对joycon(L+R)的数据
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
import ctypes, os, sys, time, threading
import numpy as np
# ---------- 1. 载入 hidapi ----------
DLL_PATH = os.path.join(os.path.dirname(__file__), 'hidapi.dll')
if os.path.exists(DLL_PATH):
ctypes.CDLL(DLL_PATH)
# ---------- 2. Joy-Con 连接 ----------
from pyjoycon import JoyCon, get_L_id, get_R_id
def connect_joycon(get_id_func, side='L', retry_delay=1.0):
while True:
try:
joycon_id = get_id_func()
return JoyCon(*joycon_id)
except Exception:
print(f'[INFO] 等待 Joy-Con-{side} 连接中 …')
time.sleep(retry_delay)
# ---------- 5. 数据采集线程 ----------
def joycon_thread(vc: DualJoyConVisualizer, side='L'):
get_id_func = get_L_id if side=='L' else get_R_id
joycon = connect_joycon(get_id_func, side)
filt = ComplementaryFilter(alpha=0.98, gyro_scale=0.15)
print(f'[INFO] Joy-Con-{side} 已连接！')
while True:
status = joycon.get_status()
accel = np.array([status['accel'][c] for c in ('x','y','z')], dtype=float)
gyro = np.array([status['gyro'][c] for c in ('x','y','z')], dtype=float)
now = time.perf_counter()
rot = filt.update(accel, gyro, now)
if side == 'L':
vc.rotL = rot
vc.buttons_L = {btn: val for sideb in status['buttons'].values() for btn, val in sideb.items()}
a_world = rot.apply(accel)
a_world = a_world - np.array([0,0,9.8])
vc.motion_buffer_L.append((now, a_world))
else:
vc.rotR = rot
vc.buttons_R = {btn: val for sideb in status['buttons'].values() for btn, val in sideb.items()}
a_world = rot.apply(accel)
a_world = a_world - np.array([0,0,9.8])
vc.motion_buffer_R.append((now, a_world))
time.sleep(0.01)
if __name__ == '__main__':
app = QtWidgets.QApplication(sys.argv)
vis = DualJoyConVisualizer()
tL = threading.Thread(target=joycon_thread, args=(vis,'L'), daemon=True)
tR = threading.Thread(target=joycon_thread, args=(vis,'R'), daemon=True)
tL.start()
tR.start()
vis.show()
sys.exit(app.exec_())
空间运动
https://github.com/tocoteron/joycon-python/issues/31
可能的参考
https://github.com/Looking-Glass/JoyconLib
demo
通过线性积分和降低系数来获得了一个大概的 joycon->朝向 的功能
需要解决的问题：
- 通过特定的按键初始化与现实世界坐标系的校准（如垂直向下/水平等）
- 旋转的转换还有些问题，会产生突变
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
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
import ctypes, os, sys, time, threading
import numpy as np
from scipy.spatial.transform import Rotation as R, Slerp
# ---------- 1. 载入 hidapi ----------
DLL_PATH = os.path.join(os.path.dirname(__file__), 'hidapi.dll')
if os.path.exists(DLL_PATH):
ctypes.CDLL(DLL_PATH)
# ---------- 2. Joy-Con 连接 ----------
from pyjoycon import JoyCon, get_L_id, get_R_id
def connect_joycon(get_id_func, side='L', retry_delay=1.0):
while True:
try:
joycon_id = get_id_func()
return JoyCon(*joycon_id)
except Exception:
print(f'[INFO] 等待 Joy-Con-{side} 连接中 …')
time.sleep(retry_delay)
# ---------- 3. 互补滤波 ----------
class ComplementaryFilter:
def __init__(self, alpha=0.98, gyro_scale=0.15):
self.alpha = alpha
self.gyro_scale = gyro_scale
self.rot = R.identity()
self.last_t = None
def update(self, accel_raw, gyro_dps, t):
G = 9.8
if self.last_t is None:
self.last_t = t
return self.rot
dt = t - self.last_t
self.last_t = t
# Gyro 积分（已知 gyro_dps 为度/秒，需转弧度）
# 注意：不要重复转换单位！
delta_rot = R.from_rotvec(self.gyro_scale * np.deg2rad(gyro_dps) * dt) # gyro_dps为度/秒
self.rot = self.rot * delta_rot
# 判断是否静止
is_static = (
np.abs(np.linalg.norm(accel_raw) - G) < 0.3 and
np.linalg.norm(gyro_dps) < 3.0
)
# 只有静止才用重力校正
if is_static:
gx, gy, gz = accel_raw / np.linalg.norm(accel_raw)
pitch = np.arctan2(gx, -gz)
roll = np.arcsin(gy)
gravity_rot = R.from_euler('yx', [pitch, roll], degrees=False)
# 用线性插值近似球插值
rot_quat = self.rot.as_quat()
gravity_quat = gravity_rot.as_quat()
interp_quat = self.alpha * rot_quat + (1 - self.alpha) * gravity_quat
interp_quat /= np.linalg.norm(interp_quat)
self.rot = R.from_quat(interp_quat)
return self.rot
# ---------- 4. 可视化 ----------
import pyqtgraph as pg
import pyqtgraph.opengl as gl
from PyQt5 import QtCore, QtGui, QtWidgets
# 设置运动检测窗口（秒）
MOTION_WINDOW_S = 5.0
class DualJoyConVisualizer(QtWidgets.QWidget):
def __init__(self):
super().__init__()
self.setWindowTitle('Dual Joy-Con 棍子 3D 视图（双端运动方向箭头）')
self.resize(900, 600)
self.glv = gl.GLViewWidget()
self.glv.opts['distance'] = 300
layout = QtWidgets.QVBoxLayout(self)
layout.addWidget(self.glv)
axis = gl.GLAxisItem(size=QtGui.QVector3D(80, 80, 80))
self.glv.addItem(axis)
verts = np.array([
[-40,-2,-2], [40,-2,-2], [40,2,-2], [-40,2,-2],
[-40,-2,2], [40,-2,2], [40,2,2], [-40,2,2],
])
faces = np.array([
[0,1,2], [0,2,3],
[4,5,6], [4,6,7],
[0,1,5], [0,5,4],
[2,3,7], [2,7,6],
[1,2,6], [1,6,5],
[0,3,7], [0,7,4],
])
colors = np.ones((faces.shape[0], 4), dtype=float) * [0.5,0.8,0.3,0.8]
meshdata = gl.MeshData(vertexes=verts, faces=faces, faceColors=colors)
self.stick = gl.GLMeshItem(meshdata=meshdata, smooth=False, shader='shaded', drawEdges=True)
self.glv.addItem(self.stick)
# L端箭头（红）
self.arrow_mesh_L = self.make_arrow_mesh()
self.arrow_item_L = gl.GLMeshItem(meshdata=self.arrow_mesh_L, color=(1,0,0,1), smooth=True, shader='balloon', drawEdges=False)
self.arrow_item_L.setGLOptions('additive')
self.glv.addItem(self.arrow_item_L)
# R端箭头（蓝）
self.arrow_mesh_R = self.make_arrow_mesh()
self.arrow_item_R = gl.GLMeshItem(meshdata=self.arrow_mesh_R, color=(0,0.4,1,1), smooth=True, shader='balloon', drawEdges=False)
self.arrow_item_R.setGLOptions('additive')
self.glv.addItem(self.arrow_item_R)
self.label = QtWidgets.QLabel(self)
self.label.setStyleSheet("color: white; background: transparent; font-size: 18px;")
self.label.setGeometry(10, 10, 900, 50)
self.label.setAttribute(QtCore.Qt.WA_TransparentForMouseEvents)
self.timer = QtCore.QTimer()
self.timer.timeout.connect(self.update_stick)
self.timer.start(15)
self.rotL = R.identity()
self.rotR = R.identity()
self.buttons_L = {}
self.buttons_R = {}
self.motion_buffer_L = [] # [(timestamp, a_world)]
self.motion_buffer_R = [] # [(timestamp, a_world)]
def make_arrow_mesh(self):
cylinder = pg.opengl.MeshData.cylinder(rows=10, cols=20, radius=[0.7,0.7], length=6)
cone = pg.opengl.MeshData.cylinder(rows=10, cols=20, radius=[1.2,0], length=3)
c_verts = cylinder.vertexes() + np.array([3,0,0])
c_faces = cylinder.faces()
cone_verts = cone.vertexes() + np.array([6,0,0])
cone_faces = cone.faces() + len(c_verts)
verts = np.vstack([c_verts, cone_verts])
faces = np.vstack([c_faces, cone_faces])
return gl.MeshData(vertexes=verts, faces=faces)
def update_arrow(self, direction, arrow_item, offset):
norm = np.linalg.norm(direction)
if norm < 1e-2:
arrow_item.setVisible(False)
return
arrow_item.setVisible(True)
scale = min(norm * 2, 20)
v = direction / norm if norm > 1e-6 else np.array([1,0,0])
x = v
z = np.array([0,0,1]) if abs(np.dot(x,[0,0,1]))<0.99 else np.array([0,1,0])
y = np.cross(z, x); y /= np.linalg.norm(y)
z = np.cross(x, y)
rot = np.eye(4)
rot[:3,0] = x
rot[:3,1] = y
rot[:3,2] = z
rot[:3,3] = offset
mat = QtGui.QMatrix4x4(*rot.T.flatten())
mat.scale(scale, scale, scale)
arrow_item.setTransform(mat)
def calc_motion_vector(self, buf):
if not buf: return np.zeros(3)
v = np.zeros(3)
prev_t = buf[0][0]
for t, a in buf:
dt = t - prev_t
v += a * dt
prev_t = t
return v
def vec_to_dir(self, v):
n = np.linalg.norm(v)
if n < 1e-6:
return "静止"
vn = v / n
x, y, z = vn
# 上下优先
if abs(z) > 0.7:
return "上" if z > 0 else "下"
angle = np.arctan2(y, x)
dirs = [
"前", "右前", "右", "右后",
"后", "左后", "左", "左前"
]
idx = int(((angle + np.pi) / (2*np.pi)) * 8) % 8
return dirs[idx]
def power_level(self, v):
force = np.linalg.norm(v)
if force > 30:
return "重挥动"
elif force > 10:
return "中等挥动"
elif force > 3:
return "轻挥动"
else:
return "静止/极轻"
def update_stick(self):
# 平均姿态
rots = R.from_quat([self.rotL.as_quat(), self.rotR.as_quat()])
times = [0, 1]
slerp = Slerp(times, rots)
avg_rot = slerp(0.5)
quat = avg_rot.as_quat()
mat = QtGui.QMatrix4x4()
q = QtGui.QQuaternion(quat[3], quat[0], quat[1], quat[2])
mat.rotate(q)
self.stick.setTransform(mat)
now = time.perf_counter()
# 保留略多于窗口长度的数据
self.motion_buffer_L = [(t, a) for (t, a) in self.motion_buffer_L if now-t < MOTION_WINDOW_S+0.2]
self.motion_buffer_R = [(t, a) for (t, a) in self.motion_buffer_R if now-t < MOTION_WINDOW_S+0.2]
# L端
motion_vec_now_L = self.calc_motion_vector([(t,a) for (t,a) in self.motion_buffer_L if now-t<MOTION_WINDOW_S])
motion_vec_past_L = self.calc_motion_vector([(t,a) for (t,a) in self.motion_buffer_L if MOTION_WINDOW_S<=now-t<MOTION_WINDOW_S+0.1])
# R端
motion_vec_now_R = self.calc_motion_vector([(t,a) for (t,a) in self.motion_buffer_R if now-t<MOTION_WINDOW_S])
motion_vec_past_R = self.calc_motion_vector([(t,a) for (t,a) in self.motion_buffer_R if MOTION_WINDOW_S<=now-t<MOTION_WINDOW_S+0.1])
# 箭头显示
self.update_arrow(motion_vec_now_L, self.arrow_item_L, np.array([-40,0,0]))
self.update_arrow(motion_vec_now_R, self.arrow_item_R, np.array([40,0,0]))
# 方向和力度文字
str1_L = self.vec_to_dir(motion_vec_past_L)
str2_L = self.vec_to_dir(motion_vec_now_L)
power_L = self.power_level(motion_vec_now_L)
str1_R = self.vec_to_dir(motion_vec_past_R)
str2_R = self.vec_to_dir(motion_vec_now_R)
power_R = self.power_level(motion_vec_now_R)
motion_txt_L = f"L端: 从{str1_L}到{str2_L}，{power_L}"
motion_txt_R = f"R端: 从{str1_R}到{str2_R}，{power_R}"
txtL = ','.join(f'L:{k}' for k,v in self.buttons_L.items() if v)
txtR = ','.join(f'R:{k}' for k,v in self.buttons_R.items() if v)
txt2 = (txtL + ' ' + txtR) or 'No button pressed'
self.label.setText(f"过去{MOTION_WINDOW_S:.0f}s {motion_txt_L} {motion_txt_R} {txt2}")
# ---------- 5. 数据采集线程 ----------
def joycon_thread(vc: DualJoyConVisualizer, side='L'):
get_id_func = get_L_id if side=='L' else get_R_id
joycon = connect_joycon(get_id_func, side)
filt = ComplementaryFilter(alpha=0.98, gyro_scale=0.15)
print(f'[INFO] Joy-Con-{side} 已连接！')
while True:
status = joycon.get_status()
accel = np.array([status['accel'][c] for c in ('x','y','z')], dtype=float)
gyro = np.array([status['gyro'][c] for c in ('x','y','z')], dtype=float)
now = time.perf_counter()
rot = filt.update(accel, gyro, now)
if side == 'L':
vc.rotL = rot
vc.buttons_L = {btn: val for sideb in status['buttons'].values() for btn, val in sideb.items()}
a_world = rot.apply(accel)
a_world = a_world - np.array([0,0,9.8])
vc.motion_buffer_L.append((now, a_world))
else:
vc.rotR = rot
vc.buttons_R = {btn: val for sideb in status['buttons'].values() for btn, val in sideb.items()}
a_world = rot.apply(accel)
a_world = a_world - np.array([0,0,9.8])
vc.motion_buffer_R.append((now, a_world))
time.sleep(0.01)
# ---------- 6. 主程序 ----------
if __name__ == '__main__':
app = QtWidgets.QApplication(sys.argv)
vis = DualJoyConVisualizer()
tL = threading.Thread(target=joycon_thread, args=(vis,'L'), daemon=True)
tR = threading.Thread(target=joycon_thread, args=(vis,'R'), daemon=True)
tL.start()
tR.start()
vis.show()
sys.exit(app.exec_())
本文由作者按照 CC BY 4.0 进行授权