
    |'j(                     X    S r SSKrSSKrSSKJr  SSKJr  S rS r\	S:X  a  \" 5         gg)u  
R1 零力矩模式 & 阻尼模式 测试脚本

R1 与 G1 共用 unitree_hg 接口，直接复用 G1 的 LocoClient。

⚠️ 安全警告：
- 测试前必须将 R1 悬挂或安排人扶住！
- ZeroTorque() 和 Damp() 会让机器人失去主动支撑，直接摔倒！
- 首次测试建议先运行 CheckMode() 确认通信正常。

使用方式：
    python3 test_r1_torque_damp.py <网卡名>
    例：python3 test_r1_torque_damp.py enx000e0988a670
    N)ChannelFactoryInitialize)
LocoClientc                      [        S5        [        S5        [        S5        [        S5        [        S5        [        S5        [        S5        [        S5        [        S	5        [        S5        g )
Nz)
========================================u"   R1 零力矩 / 阻尼模式 测试z(========================================uO     [0] ZeroTorque  — 零力矩模式（电机无力矩，机器人会瘫倒）u=     [1] Damp        — 阻尼模式（关节可被动推动）u/     [2] Start       — 恢复站立/正常状态uE     [3] Squat2StandUp — 蹲姿→站立（如果机器人已倒地）u,     [4] Check FSM ID — 查询当前状态码u     [q] Quit        — 退出)print     test_r1_torque_damp.py
print_menur
      sW    	/	
./	(O	
[\	
IJ	
;<	
QR	
89	
()	(Or   c                     [        [        R                  5      S:  aV  [        S[        R                  S    S35        [        S[        R                  S    S35        [        R                  " S5        [        R                  S   n [        S5        [        S	5        [        S
5        [        SU  35        [        SU 5        [        5       nUR                  S5        UR                  5       nUS:w  aP  [        SU 35        [        S5        [        S5        [        S5        [        S5        [        R                  " S5        [        S5         UR                  5       u  p4US:X  a  [        SU 35        O[        SU 35          [        5         [        S5      R                  5       R                  5       nUS:X  a=  [        S5        UR                  5         [         R"                  " S5        [        S5        g US:X  aq  [        S5        [        S5        [        S5      R                  5       R                  5       nUS:X  a!  UR%                  5       n[        S U S!35        GO[        S"5        GOUS#:X  aq  [        S$5        [        S%5        [        S5      R                  5       R                  5       nUS:X  a!  UR'                  5       n[        S U S!35        GO[        S"5        GO	US&:X  aA  [        S'5        UR                  5       n[        S U S!35        [         R"                  " S(5        OUS):X  aL  [        S*5        [        S+5        UR)                  5       n[        S U S!35        [         R"                  " S(5        OpUS,:X  a_   UR                  5       u  p4US:X  a7  [        S-U 35        S.S/S0S1S2S3.nUR+                  US45      n	[        S5U	 35        O[        S6U 35        O[        S85        [         R"                  " S95        GM  ! [         a$  n[        SU 35        [        S5         S nAGNS nAff = f! [         a  n[        S7U 35         S nANgS nAff = f):N   u   用法: python3 r   u    <网卡名>u   示例: python3 z enx000e0988a670   u@   
⚠️  安全确认：请确保 R1 已悬挂或有人扶住！uE       零力矩/阻尼模式会导致机器人失去支撑并摔倒。
u"   确认安全后按 Enter 继续...u   
[初始化] 网卡: g      $@u-   [错误] LocoClient Init 失败，返回值: u   可能原因：u:     1. R1 未进入开发者模式（遥控器按 L2 + R2）u4     2. 网络不通（检查网线、IP、网卡名）u#     3. R1 不支持 LocoClient 接口u=   [成功] LocoClient 初始化完成，与 R1 建立通信。
u   [当前状态] FSM ID: u$   [警告] 查询状态失败，code: u    [警告] GetFsmId 调用异常: u>   继续运行，但 R1 可能不支持全部 LocoClient API。
u   输入选项: qu   
退出前恢复站立状态...u   已退出。0u1   
>>> 正在进入 零力矩模式 (ZeroTorque)...u1       电机将完全断电，机器人会瘫软！u       确认执行? [y/N]: yu       调用完成，返回值: u    (0=成功)u       已取消。1u(   
>>> 正在进入 阻尼模式 (Damp)...u1       电机提供阻尼，关节可被动推动。2u"   
>>> 正在恢复站立 (Start)...g      ?3u4   
>>> 正在执行 蹲姿→站立 (Squat2StandUp)...uD       注意：此操作仅在机器人处于蹲姿/倒地时有效。4u   
[当前 FSM ID]: u   零力矩 (ZeroTorque)u   阻尼 (Damp)u   站立/正常 (Start)u   躺→站 (Lie2StandUp)u   蹲→站 (Squat2StandUp))r   r   i  i  i  u   未知状态u   [状态含义]: u   
[错误] 查询失败，code: u   
[错误] 调用异常: u"   
无效选项，请重新输入。g333333?)lensysargvr   exitinputr   r   
SetTimeoutInitGetFsmId	Exceptionr
   striplowerStarttimesleep
ZeroTorqueDampSquat2StandUpget)
network_interfaceclientretcodefsm_idechoiceconfirmfsm_mapdescs
             r	   mainr1   %   s   
388}q !\:; !-=>?	
MN	
RS	
./ 
"#4"5
67Q 12 \F
d
++-C
ax=cUCD JKDE34	
JKQ(19+F8458?@
 '(..0668S=45LLNJJqM.!s]FGEF78>>@FFHG#~((*6tfKHI()s]=>EF78>>@FFHG#~{{}6tfKHI()s]78<<>D24&DEJJsOs]IJXY'')D24&DEJJsOs]7%019/x89 4*468G #;;v~>D,TF34<TFCD
 78

3G 	  Q045OPPQB  71!5667s=   &P
 6P
 AP; P; 

P8P33P8;
QQQ__main__)
__doc__r   r!   unitree_sdk2py.core.channelr   %unitree_sdk2py.g1.loco.g1_loco_clientr   r
   r1   __name__r   r   r	   <module>r7      s8      @ <
m` zF r   