发布控制指令

import time
from wuji_sdk import DeviceType, JointCommand, SdkManager

manager = SdkManager.instance()
hands = [d for d in manager.scan() if d.device_type == DeviceType.WujiHand2]
hand = manager.connect(sn=hands[0].sn, device_name="wuji_hand_2")

# 1. 配置控制参数。固件默认就是 MIT 模式，不需要单独设 control_mode
hand.effort_limit().set(1.5)
hand.mit_params().set((3.0, 0.05))

# 2. 使能
hand.enable()

# 3. 等电机真正就绪，否则指令会抢在使能完成之前
diag = hand.joint_diagnostics().subscribe()
deadline = time.monotonic() + 5.0
while time.monotonic() < deadline:
    time.sleep(0.05)
    frame = diag.recv()
    if frame and frame.joints and all(e.status_word.ext_state == 2 for e in frame.joints):
        break
diag.close()

# 4. 读当前姿态作为插值起点
sub = hand.joint_states().subscribe()
frame = sub.recv()
start = [float(j.position) if j.position is not None else 0.0 for j in frame.joints]
sub.close()

# 5. 按 200 Hz 插值到零位
publisher = hand.joint_command().publish()
steps, dt = int(1.5 * 200), 1.0 / 200
t0 = time.monotonic()
for i in range(1, steps + 1):
    a = i / steps
    target = [p * (1.0 - a) for p in start]
    publisher.send([JointCommand(v, 0.0, 0.0) for v in target])
    wait = (t0 + dt * i) - time.monotonic()
    if wait > 0:
        time.sleep(wait)

# 6. 清理
hand.disable()
publisher.close()
manager.disconnect_all()

python examples/python/wuji_hand_2/2.publish.py