From 28a2138dd7141528873f4acf7a34261ac248c007 Mon Sep 17 00:00:00 2001 From: Ziwei Date: Thu, 29 May 2025 21:40:02 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9A=82=E5=AD=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Massage/MassageControl/MassageRobot_nova5.py | 7 ++++--- .../__pycache__/dobot_nova5.cpython-39.pyc | Bin 23567 -> 23700 bytes .../__pycache__/force_sensor.cpython-39.pyc | Bin 11720 -> 11720 bytes .../MassageControl/hardware/dobot_nova5.py | 10 ++++++---- 4 files changed, 10 insertions(+), 7 deletions(-) diff --git a/Massage/MassageControl/MassageRobot_nova5.py b/Massage/MassageControl/MassageRobot_nova5.py index b6db9cd..b87518d 100644 --- a/Massage/MassageControl/MassageRobot_nova5.py +++ b/Massage/MassageControl/MassageRobot_nova5.py @@ -570,6 +570,7 @@ class MassageRobot: time.sleep(sleep_duration) print(f"real sleep:{time.time()-b2}") self.arm.dashboard.socket_dobot.sendall(tcp_command) + print("currentCommandID",self.arm.feedbackData.robotCurrentCommandID) if self.arm.feedbackData.robotMode == 10: # 10 当前工程暂停 print("机械臂为暂停状态") res = self.arm.dashboard.Continue() @@ -589,7 +590,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() @@ -1608,7 +1609,7 @@ if __name__ == "__main__": # robot.dt = ts[1] - ts[0] - ready_pose = [194.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042] + ready_pose = [204.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042] robot.current_head = 'finger_head' robot.force_sensor.disable_active_transmission() @@ -1629,7 +1630,7 @@ if __name__ == "__main__": atexit.register(robot.force_sensor.disconnect) - robot.arm_state.desired_wrench = np.array([0,0,-0.5,0,0,0]) + robot.arm_state.desired_wrench = np.array([0,0,-1,0,0,0]) try: robot_thread = threading.Thread(target=robot.start) robot_thread.start() diff --git a/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc b/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc index f69cf28d19d6c783b09f8588c19c0f82c38067a5..201dcee2c97cccecee8f47d71750560c9dbc6584 100644 GIT binary patch delta 4116 zcmb`K3s6+o8OQf5%XQu5DNhw8Dxw#IAV^WW;h~7gOL+*m8rKVZ*{tpZ&)!%;tVE45 z9gQW&PU0j+F=>>SUCPBYW?{LfX`Fwi)Y zvcqq`dmi8S-S2$gIcJYwA}2;jf)XF^u<*ZMvVa)Y$8RI)YGXYtNB zONI!ssB_Nb4q1rBOIj>)ihh8EV~L^vO?uWA6XT6#gU;+i=^!8HwX|5Y8Qy7kW!c^+ z6GPGz>Ql8=O1PzAV%UOVugd=9JVHuXD6T1`h&Q{PYePzBKy40phBaEieiOHZxb%O= zrCZ5jmX?@HN|`HhL@MAzX%YJ@aVdE~&r2Gy%H?QT3{(J>z-nL(PzqE54wjsfWu>(& zFQvG<9xbLWNH(AWSO_?QM&JQp0}#hCJdLJ8ZsIVKd%_Vl;tBX8nkN|acvT;5MrRAq z3T$G^k`&gT?a;5MR9fqWkHXsu^iJvMTJ%-}2p3%k)Bx*&TA+>v(=(GMGpCLwI+6)LMS=GMdZ15v!wrC#%#ts|zum(p;@DC&$xv41-Oo0^{j(uO>T$$^Pu) z#MucAUSNx8l;_>MAKNhP%lP3DiTTk?hi=Vziu-`8%%!vv-Y?^fh zX*@^QvBKQ+)gl&C)(+}hjx}pXWPwxb>hW}igFz+ajj9#>ncTBv{|*=sslOAFSQmIl z13(a{0YX3+xEDA1IN3qRL)U|elexWQyyk`9(zxGrJ0|S{#tU5k*W#DRtf^gPm-7}m zqdeppoZ)Vi;|v?N4kIS0^cb<-fv}>PYbqR@ER@384KN-ZPwYj}F6qjWi)5{a)_aU$ zpR+}!yTcpr3r2jaqGJh9{f&1M|6!=y4SWqi{<)nIHRNqpyLLB3e3J%q@>mL=H$%(B zSbmcK3X!K;@tYE#a&sDB$K=)Q*98w1im(W8rnT@hCL9h#$HyRke*Ex8k3V|Nb{E=+ z)CVPBX9LSx7mX{ijcFLRnJYhlVNvUyQ1l)lee7CsRoOvw?dOnf=If97u_>a`Xg?^l zxTQ`dqK||XTK`VpN%AyrD#vrsoF+Zlf~f3veTtDBkV~NXI$kJ11(CWpq8M zSFMsiZ~kOi`TsWGX__~_T1T#woq8LnHSfG4P(nM$UljXBX#@NdY$C%U1VN6+`%C1#> zm_61{pM~QSmZyw;Q2C)VYK%AluB^U3p)HlYyXJOMBQ#7Htly|QO~_t$q`Eb0s`}(b zP^{ms-bbXpTtnX)cC7k-cCKbtX)zQ&&tb$;CFD~LN$Kg~I|qFM9pC2QQ&SX8DfvDf1J}<+eePD!?nw{^INIyEa|s+?3(oqzkLoto-Slb$PUeU=LU+N zxPHr-_7fc= zTSETDQ`feS`p&n%Q}kw9q{7FYv{(kZM@_*}HtI_|TKi|)@?GV!)hw5OKH#p5V9>>^ zv#x_AvRCV>vkV80zi@YVMAh7qKjhauwRgTZvb6fb8Eu$>l;J8wSj?N`j(SJdT;6IS z694e6gMS?S6URSxB3bn(>f=eySX$q_!H;tzW>0Tl>o5^f9+v30p!x!Py&+FJ0r437 zYr{Hni`^oiw&VZkf8Q{jESq44Z%!{^(golWFarDpcoq05w9OUT6dHs^zvd5z z68W1(lK242bi4J*jdR()CZBzXKl1(M0{cr-t#k~tUt_N31!O+k-uwsB&&pe>d0riE zSy1qOj5IeIBZN)A_}ESFFlVY zD5c1JL-MKtRa4CuI>#DXA0&DD%dKZf^A(Iiu0)4AfM-!-?{i5`q7%$GyTYL^MN>ok z-!%0-%n(I3!cMi#bY8{tFM)IHjkY<&VAtBxIRDYMfef&5&Q3Ocq{o-~^=NY1C3{EN;6W&s(1*dt0Hxqvpn4Lk@a?EQ|E{eFlP zhyZ(lUf=-mC~z2f3=rA*6r`ttXMj^c8Sor%8aM--1=@jaz%U@P=OUyxfwwH&ZTdFE z_kiC5*MN`N=`9)T*yc?nh5d1J1KG!xxfgC7K-VL{b&ftqB@Nz(tSwq)E{M)sWy$9o z320Y3yDNN8?P`g6H(MF?J$?Cuq^+#4!guwbvkr6xdDo&_pB{SWwrkXPY3`$+G+p{` zIqP%h@HIH?UTXe`JD&`3SKZ?pUw*t&G^a9o)@Y4^E}>BC_3RJH_98OPb4JwG%u4cf{CZ M68Hv^YD;kZ7dH2iAOHXW delta 3795 zcmbuCdr(}}9mn_V($To3|zNx@%3jn(N)C;7w9bUK|eGimMl-boR#3YsZ*kV*%}R=_#wl7MAESXR$luC=>IhE|a=AY`-FVGU+$Yr8ZtJ zjMHD7$Jkm@T)T2>#bvzGz6VR;lZS!%jlW)(-P&;R zbIcin_5cvj+{^#B8197HSBc?k^xo1U%~JX+mak`BB@AA)0DAjwqw_0kO4ExDr#Q@f zzSml)wya=_UW03PJC3BsW^}v0`%ZOWG&rC{qoG)MuHUt(ve(#T1jZRK3?w7t`*1k| zYy+Y|47i>U#X66J&zDSr;zA);&Yw^{W@$cV9)!{;U>s2G!p%Qn%dQEX|v2Nh2C0|x-KX22PjW8uygdEh{Md}zUfFI;aulOC8!t@lS& z?=sJ|I^v=vmS4;O>`?U5*;RXM^g`*OWOT`@Ur|OST6}A%(?|}An>6pOH! zy36#ebocYAxeU*GI~$%U+E|p6?piz5cq4NsL!Yr2YQ{1;d)sHu^_*(clEorM?VKzGS9z--JBNby5x552Y^l4i@z0+357SX@jTJi!8Jdc#Y z!MLmjq)05H23zT_9W~qgpog6mhV<0@7`XaSC9F_5FZv-~C}vGe5+8Af#m6DOUCb^d z?c9zmR+{X-ix%c+aKHz@f8^8mA-hkhR^cZjocna7IJZ)+f zdif3lZ*-XL%r5>N7Q1DU KIL3KdcJ^;+9xl)T diff --git a/Massage/MassageControl/hardware/__pycache__/force_sensor.cpython-39.pyc b/Massage/MassageControl/hardware/__pycache__/force_sensor.cpython-39.pyc index daa2ed3f30c5c501e45e7dcb35fed9ef3361c5f8..ffa58e4310a587443dccc7927c577b2793b5ff61 100644 GIT binary patch delta 20 acmX>ReIlAWk(ZZ?0SGK_ns4OZs|x@>3I)ah delta 20 acmX>ReIlAWk(ZZ?0SF3@m~G_Vs|x@?4+ZD| diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py index c22cc70..4f479fa 100644 --- a/Massage/MassageControl/hardware/dobot_nova5.py +++ b/Massage/MassageControl/hardware/dobot_nova5.py @@ -118,7 +118,7 @@ class dobot_nova5: self.dashboard.EmergencyStop(mode=0) # 松开急停 self.clearError() # 清除报警 # 关闭碰撞检测 - self.dashboard.SetCollisionLevel(level=1) + self.dashboard.SetCollisionLevel(level=0) if self.parseResultId(self.dashboard.EnableRobot())[0] != 0: print("使能失败:检查29999端口是否被占用") return @@ -737,11 +737,13 @@ class dobot_nova5: self.last_input_command = None self.tcp_offset = None self.init_J = [-45.0009079,55.5785141,-120.68821716,5.11103201,90.00195312,-90.00085449] + self.init_P = [654.3467,-134.9880,305.3604,-180.0000,-30.0000,0.0042] self.filter_matirx = np.zeros((1,7)) # 位姿伺服用 self.filter_matrix = np.zeros((1,6)) # 角度伺服用 sleep(1) self.is_standby = False - code = self.RunPoint_P_inJoint(self.init_J) + # code = self.RunPoint_P_inJoint(self.init_J) + code = self.RunPoint_P_inPose(self.init_P) if code == 0: print("机械臂初始化成功且到达初始位置") else: @@ -881,7 +883,7 @@ if __name__ == "__main__": # print(dobot.get_arm_position()) - # # dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90]) + dobot.RunPoint_L_inPose([179.087702,-136.993776,425.209977,179.418397,-28.9718402,0.06]) # cur_pose = dobot.getPose(user=1,tool=8) # cur_pose[0] += 30 # target_pose = cur_pose @@ -890,5 +892,5 @@ if __name__ == "__main__": # dobot.ServoPose(dobot.getPose(user=1,tool=8)) # print("new ready joint",dobot.feedbackData.QActual) # # dobot.start_drag() - dobot.stop_drag() + # dobot.stop_drag() dobot.disableRobot() \ No newline at end of file