From 8000d3f14eefe04b5102be0ffb17798682bb513e Mon Sep 17 00:00:00 2001
From: "Ziwei.He" <hellohezw@gmail.com>
Date: Wed, 28 May 2025 10:09:15 +0800
Subject: [PATCH] update

---
 Massage/MassageControl/MassageRobot_nova5.py  |   7 ++++---
 Massage/MassageControl/config/admittance.yaml |   4 +++-
 .../__pycache__/dobot_nova5.cpython-39.pyc    | Bin 22071 -> 22931 bytes
 .../__pycache__/force_sensor.cpython-39.pyc   | Bin 10816 -> 11706 bytes
 .../MassageControl/hardware/force_sensor.py   |   2 ++
 5 files changed, 9 insertions(+), 4 deletions(-)

diff --git a/Massage/MassageControl/MassageRobot_nova5.py b/Massage/MassageControl/MassageRobot_nova5.py
index 58f8627..4ebf2fb 100644
--- a/Massage/MassageControl/MassageRobot_nova5.py
+++ b/Massage/MassageControl/MassageRobot_nova5.py
@@ -336,7 +336,7 @@ class MassageRobot:
             # 线程
             self.exit_event.clear()
             self.arm_measure_thread = threading.Thread(target=self.arm_measure_loop)
-            self.arm_control_thread = threading.Thread(target=self.arm_command_loop_traj)
+            self.arm_control_thread = threading.Thread(target=self.arm_command_loop)
             # 线程启动
             self.arm_measure_thread.start() ## 测量线程
             position,quat_rot = self.arm.get_arm_position()
@@ -496,11 +496,12 @@ if __name__ == "__main__":
     try:
         robot_thread = threading.Thread(target=robot.start)
         robot_thread.start()    
+
     except KeyboardInterrupt:
         print("用户中断操作。")
     except Exception as e:
         print("Exception occurred at line:", e.__traceback__.tb_lineno)
         print("发生异常：", e)
-    
-    
+        
+    robot_thread.join()  # 等待机械臂线程结束
     # robot.arm.disableRobot()
\ No newline at end of file
diff --git a/Massage/MassageControl/config/admittance.yaml b/Massage/MassageControl/config/admittance.yaml
index f627f9f..e04f831 100644
--- a/Massage/MassageControl/config/admittance.yaml
+++ b/Massage/MassageControl/config/admittance.yaml
@@ -8,7 +8,9 @@ mass_rot: [0.11, 0.11, 0.11]
 # P
 # stiff_tran: [300, 300, 300]
 # stiff_rot: [7, 7, 7]
-stiff_tran: [270, 270, 270]
+# stiff_tran: [270, 270, 270]
+# stiff_rot: [11, 11, 11]
+stiff_tran: [0, 0, 0]
 stiff_rot: [11, 11, 11]
 # stiff_tran: [100, 100, 100]
 # stiff_rot: [1, 1, 1]
diff --git a/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc b/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc
index cf3df8cfaf1f15c8cd4f645315d3885f39e38231..88e09f497520eed1ff9654f9705c5c7a6c87532f 100644
GIT binary patch
delta 1077
zcmZXS?@N<G9KfHuXFr~8x~4*=p@|YUHATq&fg+^M{5300Sks#`nXYa#)Sf4)6ce2U
z_af0E4J8CMb>>V{`(ZDFf?f~lm2*n3q?f+wd()=XJ>2JexBGtYZXYf<iC6#`9r5vc
z0owhcqQ<elSz`-AwM;>iY1c>&$?ZZl$s~`Q?GkEEkn1F$6u>Vjq?%pAw+d>Am6X6d
zn_MBKka32Tk#dM;Qb8&q<`5gHf|yILRtn}Dfv<SB_<SRrB@0agLGWQeR*;NpH40Wl
zMWv_}jiQw?5z_<(%VIQ&vWkdo%93(QC97qPtaXV*O*DRSL_I1fI$4r+4H)*<varRO
zO%d!rf$3uiv=^|mEeb+NY-{hZO&~hVS2fhnZd$W5CiFCsr}cAm145`hggQc)wn1+X
zu~*j2&P12fXTR<7I_(ahy}|8haXB}zAb3rXFKCR&grw+aLaC@Dop|azK6{JjuA?ab
z|3aDtHT6&(YX%r;pkILQJyav_tHCnv1UE4=igeToI07S4M`|A$0berAUE(NGbUv|3
zMO56DEdAw6FY9FezOUAHI8>i;(89s-aCdNLd2oArY;$p`@L+-MTB@s}Qt2e5(<zQ=
zfQ`Q9!x@0tu<w!UK9orp_%z6|$nlnAiDQ}L9osEVN;e&q<nwskKVxpSM#Li4^teCn
z`6{coJnFPSdF<Me;rXqB@$G?Nc=c&`g@t?ltzIWB;K||VPqs#SB7x=2#lFZ;*VeP8
zt(P;~Jr6fm{E?OE*be{xC9{T(@C%M}%yP_e1UTjan7-jxXJ4!dhP~Krbu4WCffm?$
zNftJWft?aJGNi-HquEf>QN#)>PqKTZHZ;q=m)7CqV&Gia3zYGJZ|dVXqLlDCe<}16
z`&C|;!H*B=8mOn)ey7#p3rP;I-P!7PQo7EQ-m{Ap3n-KARXmVTw9cHzl*+Z#6~4O4
zvBvR{<1@!M)~rr8P%Vs4L-$s&7TaZQf^<x8l9Jf6Z3E4*!K(hGJPGf=Z9TkjBZ@;t
F^>5h>G9&;1

delta 347
zcmbQdnQ{9XM!rN|UM>b8h_^J!Xm8)hm&eHHHMy2?jcibgR*H5DW0ZJ`PKs^|LzF~H
zaEe}vK2St5C1kQ2Q>LtUiVsj+J;gW04@hgI_@@K_Y0Z?tU<S?5&G(o#dQ2Af&z$_v
zJ4Gz>1v3M~E#`{Ks*sh8MYDnSX|fe9nB3{3&Zs$gqmLM86jM<J5LZS`zTl%YSs*}=
z_Z|}iL$LtJX>1%!lh6CfP4)@c$GB-Scc3!UJEqMBfjNv!|ClDv3#wpTH(5M5iRl~D
z=IY@2j0!t|T8c_Q#2O$GQnVJtOaT$fAY#{Kt58G6^^<c#CE0d^1h!A^4&BDcG}$Sv
zkx^^%>9G9*+d#rQLBt*qab$8mtN7&I;arS<laGaaF$;4DOcsy012p4HM2|2J2NMS)
O2yy{2A0rPVA1eTb<79pS

diff --git a/Massage/MassageControl/hardware/__pycache__/force_sensor.cpython-39.pyc b/Massage/MassageControl/hardware/__pycache__/force_sensor.cpython-39.pyc
index 945dc931df0a4bb0d4b0f1d710052ed4e2a04ae8..304bf5a99943a0c6720b87edd3d69ea6cfb15529 100644
GIT binary patch
delta 3301
zcmZ`*Yiv{39Y5##{fO;6AS6zL^Nyh;;T2J&@-7qxDQ)OdE^3YAdy?2Vc6#nLg*2(R
zCbU3N13eLl!iwE#Q#-biHE48DZPPyN%l2i`v=8^grfE13P1>ZPY8o2q{{Q0;MxeI*
zyZ>|kkMn<?>t7$ZdB_(nDRC3{cz(Zruwm0VUk&}@+{$VP)rdxumUDa8WF_TMJ@V;Q
zz08v+$@!QU&@VB-yt&dm(UiQBr+G@rE}~g9>jcrP<BD3w$~D^&<t*jbAG1`UR?J&k
z!YXo0S!JFkmL-;rOA8=rPgV_8tJO41q*_o7K2&Gbnq!=*HLPr$sI{6?b4?Jnjs<f;
z7ECPH+?r>C<P}PoBNx)V=n$*T)#u4mo(;fh%#(4H`35@sn3<+|GmD>KL~RDi7Of;t
z)mGN5(F}o@r&xpLn>XKjiD-V-e2IL|B6n)Qy#Ts*mcB8p(H;^mox@Uek1X96)%EBA
z>wu^DJ8kw&!!{!-R7F*^u&7gNLd7^$J9B7Wtz-gXP@T-B`7Tq<uLUNoLRHjetyOD-
z<Sc!USczH*C#*1CuO?zWETyM;_Z;~RC3E-1pOsoVCH|}I+BQlOL_aV}awT~(N+M)r
z8PGnA+3@EAz%508dr7LnN{q7OA5h=`uUwN6fo!SBSk}`Dan<rkX`nxz(jvM%D>9ts
zhS+P}6ShMr+=1W(_>Ax%Jnp~JF_<1=9SOa^PeSBPU3_WXXD<V$-X#3CD$!)SM^}ku
z_HoZ66JGH{dtG)t3Ml}Dj+8Eh*B(o!QY>b~D01dMcqaDycKGQgz{n>W9is*rrJ6FX
zOjeI62F)p>(8*(#Ug%>hWNzF-Nh+|K=$)N1UP=zabJUW9E(f`+)v#)|Q3{1su&{<L
zVNW=6&O8~jjoNmQ*Vc{M4eO{q;X>}Hq>d=O547yK`%n*Pw*rQHNe_7gxzOb@*=r!#
zp5uR{WXwUyNHuVrqmGAKy(D`O{2X(<?MS(c&rZ?_J5X&`rWHagT)Q=U>X(IUzbM?C
zDxCPqom<xnr!U?9GB-PMrEu}nJ0DHl{^Ik(<o8F`JXwQ|fn>Tbnv6gy`{M&0{b?R!
z^A&AB9vw=Wly{rirACwwFe8%D`yv`U8V8x-s4<vn$HQ-YK6~<P;q;Zl^t+*_%Y|ld
z+$x;T6;4ivGL4{u&Yqlp($m7_Pw!m42A*yVW!9qH<sV7WEXdxNy?ko+%zK5Oel$Bd
zdG}Y-5Y>|g{`Q@BGPS69<p-cxjC%IM&kK|1?o3^oo4R=S+O>_Lu-CMvhgr(BVt}ek
zkM^@jG9A@S3&hB)K%cJwFl|OUozzu#G8@+o+26YAjHQRL({<IqC>f@`m>JXc{IM7t
zHsa}2*vYHGkm|#OBK`4XF>R(5Nz<xDAziCbz$Q&{Y<rV3IWR@2fgN7H3vdJc^bi11
z6vb-s(l(1*@hM*Fq;B{IsE>N#*F;CE9}UQ&`4oS(ze{am(6Phj0z)s0pE)+!I1ZQi
z$`Px>Bj&ya2vd$D6t+tDr2yX~jyjjq!!zfdn-#F}C-*_RL3Da@v_$;D(@MSKKc3Ch
zBcAoHY}y98#SHp;7>`Gj3ki`CaxAU*!27z5cOmspZxtPwah2SoVE2KqmTnh8e}iWS
z@aC*&YNvR?ziu-of$s(gmvb-DdlB{_bR)co;6d1r@Djqy2t5ef#4UfF{Q#0{#b5l(
zYkQG=72zO&>6yR9WKFt6Fi=lFo_Q|tq2im<P{;-_66#zn4wqNDIzwBxg}S!#?czb%
z1{X&s=Xq74xBM#IDJ&Ix=q}M+(U^S!wWZsoyaeez2;V}$t7@^wvimSR(-GGr*rqa*
zTzW1emNCf-67$Gk0SMcQh59z?IeFAzk=T&NRah##OZ=;%o!*?OU-E#`xL97f3Ow6a
zS-F2Fs!Pv2|Ch1_2Y3ScI0)Z(c-T@r_XrAgiR+ae^xreLD=%XqzgyMPjV0uG%t%RW
zkj0LK`lHbJT0O^FhP}lyE)>tS#8Zap?&s;DNZ)aT>9TUBogFh+N`nf1Rn@Q&^ANTd
zkA1v^ydO`)j#@lF*cs(?=A~*6?R*_*)1HhQMv@%}D?BaNdVC<oG+64WZUBT8Q)%zy
z&xtd^M*57H4sPCvs+=Pn1{i=Je44fti7k^`AREuZ4wwDHfC{^ce!eBbHDUh<^5i@p
z1#0C?w5CDvVn=jKdn4yXzHUNH*OfVB%D7iNvpkWNEhD={+J4m9as*_FupvyV5g%fv
zO;0j5EC)>9W2UV?nb8Lyjf!cDCDS^4*f5(xRdqt^!&}_oyjVxrTgaey-sMWzTqo*7
zpV8N5eitgIOCGhdY#1LZ1;oAj-8O8<!(wa0*MY?{Vt-?u4P!~o=#4MZq*TK6neMnA
zPw7T96=O1erX!_AIgcLaWoX!sP=O#@UY__iF0%j+n-H(Dr<VT&Fy0dF&A*lZHMn2g
zZw}M<MN`X)l5TE6EgazRG7)K6dx)0<ZF<2MBOV)K#$a0GcvluDMBZ$NaEWg7B*GAa
zynL{(95cn=LBK>52OT}&?~A)FE6YoP)LQ`H+75c1PK&czG_|gyR^e-_6-Qf7`7go0
z867<$aJBObu-~;rz=}@+KwH|eSu;cYyg1la6ZjaU@1Zcih+q>Z+O}rfcpolscGXH<
zUcv41UDlplc$1knFfx3+*sV)YJb)n6+5wd5)-!#>JRM`YZrX?Sj1f<A^uturx@pDN
zKi`N;N3>6utCP60!z<JsdsE{ItoRwRI^6aAhmwJAN0ai>Q{BfBvH5=){DS26?B2Dz
m`+zKHF{84`GRrdG{o=>rO-{V8osbssk8seEC05~H(e*!sqgpxu

delta 2323
zcmaJ>No-q17@m2ypLaWno5arQY;9iBG*LHFC`nVgrF02N2<4I1bv={DuKgV5y)KCg
zri!IX1<|y_twpj?LG6Ve%n5O*IP`+JaX=5D3T~08IFwT*ROTOdMFm^>=Kufs_y2$M
z?R$5oL&;#!hs5uaF)*Llb|ut;Z(iwdaytkjIP1J}U}J%LGyYP5`UD0ghN!O;E+e9p
zl`<}4jP@epAkHEp&Sgcfp|!+yN?F0|hV<dpz4ENn>mGZG(7MtZT3^PQhD^h<^dL0U
zY-28}M=_GxF+KL!y5OZz;$Fsj6RlZBdNc75ZxQJ&G**hySf-Wuh`)%+3Pw~dwUGcM
zM4L<PW%QCWE^a!?Xc=_=i+<iqXBo{%H-dQK6i)Om5wM#C%UJKBT?C&-qUIuvlh9-L
zJ=YKk)2?gibBB~PiS%k9y%qfaasE3V3EdQpr%1;H=?YoLHz+M}s#XgU1-q$-1g>Ke
zB%z{HbTLe7iil?vz3(D&BE4Y>X}z}hNG3H!4KvRk1_lW|GiX_hZ%^$UT1DSuv`YA@
z(u#}R?by3<DS952f>4JQDuvIZjLhX7WOO=_RlE2h7B}Lqj>v3<ZVg{?Y{3zJ*YO2k
za*nTci)@((z$@SuV$I@sI5{|<&(Xn*Id?(=|HFAutr0h7oR7L<eAe|R?&Sy6W&bn#
zAh*>vWeD;1?s^_}d-!X<B3Ahw-!2y{<0kI)kEphjB&{Sr;7_#;2;u4@0UBn0IGn6W
z9Bd1~CjO(p9gkJ+`$rYwWFYh|9^e;4B^=_t;dMC3C&HsRz*oZkc$j||ZVE#lws-fr
z6kV{=d4t~zA8WNSmE}~S1vES+wGvB|<@Z%)YaU?XdZMlsZ{tQ?r~g%<u<C^9+xfM+
z&7-daIVwPluprPo0p0-E1+W_+0I&yOFTg&4ae!eSinQ(A59FHyY{gixT}j50i<(;&
zs=HJ9TrO!4b_kRX15ALX-;|?IK&RLwHzFOlP+5swRou)4N-Dq9(B>VAkL`?a9b?=0
zM8mMM`cMdez}xvu<9_@)FE@4-MnG4lU&}&3?*JGBfKmTD^BBmWOxv9{6Zr*dus#sU
z3S{N7HaTx7h#e51xvDce2zDNpv}hugBkV1~DkAmW(GB=o#ftuoagwv90g+j$sc~XE
zn9I!k&$aGp0_+6b|8H>3QH`4d%}MTV9>hOaUTMA#1O2IG?I9Q?gD^|M)I3d{iq9p}
zSxPz>%v1|h$N6-Kwj*s=wr`H*bBPm+7B%GrZIzz0s2I;M-`CpN0e#TaYS<SC#*RU>
zN&cG}QDG<kLX{h>empcIXj{#uEh|f>HHDp&yqP{}P$Jk<V3if1DYmj<h;8Ke+PcJM
zJK9J6V9GK8NBF_^XyXPTX9121(7e?&On$yy3o{VIax+6fpz=+7r{acz9}>pDc3f9w
zyMw&YnJIv{x&qSUv%Qu3E&C2rww+cwM{SpxrF22AFN5jWuDR@KbN<<PV7pS;yh)#I
z!DgYOi!aO|3DaV%Ivj`yZwdgvuex20f~n_L*DX9!Y3q*QdYRM?P?0MT2CA0t=^1yy
zijDEldLG4IqGLNV`LuDhdyOh9I$XKC_As87)(K%}`_g9GFs-DKqOx_i+aU7$V-bh|
zuNhkdAh%hDJ_GDo0Z;ZzzD07ft`CHGmS5<-t5(hVK~2M1epc(NWe%{DF9&>}S|k5j
z+caYbL~tve%28`RPnZvMWW$GnngW1483CXGusm{z40^<t0H6ici9rHvnQ!mwuZ1Gb
zZUG+biv~O%2Vd&jjF0o%eP4yoV{r$$And{HvXHg%3;nVBkA;MNB0#L03QKr8$3Eq^
z`&()X!r}oKuqyy6|D}JdYc11(fv?bZ3EhRo>i*S(v=-nDKe|3T1a565Z`#g<q&3e5
zfpsTOn3DSlIQ1m8QfCQM1jCB_%k^7#Uy^c2BAJzsk?uQ}NhPZ99=j}Wr^fe=ADWia
nsCGwANOny2?ih~`jKn8}u16I5_^&*nIvt?`Dxf)D9N78~&M^-6

diff --git a/Massage/MassageControl/hardware/force_sensor.py b/Massage/MassageControl/hardware/force_sensor.py
index 6494472..b62ca4c 100644
--- a/Massage/MassageControl/hardware/force_sensor.py
+++ b/Massage/MassageControl/hardware/force_sensor.py
@@ -1,9 +1,11 @@
 import struct
 import serial
+import serial.tools.list_ports
 import numpy as np
 import atexit
 import threading
 import time
+import yaml
 
 import subprocess
 import psutil