From e1583b1756993361d5dfed343bc6ba1db85b4cc1 Mon Sep 17 00:00:00 2001 From: "Ziwei.He" Date: Mon, 26 May 2025 13:38:58 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96STEP=E7=AE=97=E6=B3=95?= =?UTF-8?q?=EF=BC=8C=E9=99=8D=E4=BD=8ECPU=E4=BD=BF=E7=94=A8=E7=8E=87?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../admittance_controller.cpython-39.pyc | Bin 3299 -> 3315 bytes .../algorithms/admittance_controller.py | 131 +- logs/MassageRobot_nova5_test.log | 1432 ++--------------- 3 files changed, 239 insertions(+), 1324 deletions(-) diff --git a/Massage/MassageControl/algorithms/__pycache__/admittance_controller.cpython-39.pyc b/Massage/MassageControl/algorithms/__pycache__/admittance_controller.cpython-39.pyc index 0d9540de7a693e8f940e89c74e3fcdcf5c6cdb2a..0a77fb66b4751f5279fb997809cc06d1fa370d7a 100644 GIT binary patch delta 902 zcmYLG&ui2`6rPzRv&kgQrn}p9cWtq@rP?1LYM~wqiq?}zJ$TwnSWIXex4Z2$wwGD= zR0Kgm2Ek+E#j}Xu#dH4y4|^5=0P&_EzL|7u;Jx>~H*dc0z0CLKucqIbm|zILZ$EDC z{G9&i*9_eKG;gfnJBgP~2$Z-v7AW*nBXhu*ums&EIo8$kdDRxqc{PZ9xG^c8n_qE7 zZdhAY+THDSy4xdTw|6wM9)=L)OKTsm%0Jc;4&*#t8+tGV1-l1!_yr>b+9<&p(b33p zgr6e5P-Lz?4UN>ytnvAP2$Inp4Gdi=X>T{sz!XN}2s0=cS%e~TLN*X|yEz&&R1#RJ zh6a|f5-&0$^A(oA>7^kBnzE(}w4ZFC01Xs*Mdu7M8&FkHQEDIof`w366DXU~J+;Wn z3D)u^N(!Eu0JU{QPzd>1Ul38%m)5fjqF5pr7SlG7YCqc`h)9G^NKsRNec)&cCo1Xd zX4VkSxF^S16aEPkL~qLUlCE%7RSg%ml!vieX+61^P3wVMIi69YwM0ohLPG4d|4hjbBteBcu}3dbFNwZszRsE z%+TXLz!8bkw%6f%VW01Y2hTcb+Z|_nkd)JSZ$JF6)b@BkRiZ^NJLYlk$&M4VMi zDDJjP5Y>e?i{ftDDX?GQbCDnzt0>K|7az+6Cc|oz;(ba6|oN% zkOr6usPdPZp6bJ%e9KEv>cGKJeLsAO#mb7E;iKs~`-)iEU!jl$)U~azj>K zxX{i012e7#5pgBr&k(Y8-;IC3bMK@T>cBmZ@0{osiyZf8!aWiB_kxl41%fs?p|NP*hixj@T_k2PO>(0{;`*fE}o&qg2C#e%se zo|>B@2jvYs5Ah7nfDlf)q>FlJX+(xpMOvmSZ{(1`8^X4V%v0U5k(!xxUeD~DbRABE zLdi`?LQm;7`M*aZ%Y}Rnk`|DmhM>o_9J-}MOZ4|J)VVHvYhh%hQ{5n&#{LSfYoMP| z+)&xs2-%UmvZ``dX2l3`jgu@=Xe}{v3dLYG4q*|6HRWSen)HQ85|4pg(G#G~Hv?K6te!>rFRl@24$AZI+9;&pL|cwOB8nBy2`bp-d<*)d%Ul z_Td_agLO2723i$-ST 5: - # print(f'temp_pose_error: {temp_pose_error} ||| arm_position: {self.state.arm_position} ||| desired_position: {self.state.desired_position}') + # 方向统一 if self.state.desired_orientation.dot(self.state.arm_orientation) < 0: self.state.arm_orientation = -self.state.arm_orientation - self.state.pose_error[:3] = R.from_quat(self.state.arm_orientation).as_matrix().T @ temp_pose_error - # if time.time() - self.laset_print_time > 5: - # print("pose_error:",self.state.pose_error[:3]) - # 计算误差 位置直接作差，姿态误差以旋转向量表示 - #rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ R.from_quat(self.state.desired_orientation).as_matrix().T - rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix().T @ R.from_quat(self.state.desired_orientation).as_matrix() - # print(f'rot_err_mat: {rot_err_mat} ||| arm_orientation: {R.from_quat(self.state.arm_orientation).as_euler('xyz',False)} ||| desired_orientation: {R.from_quat(self.state.desired_orientation).as_euler('xyz',False)}') - rot_err_rotvex = R.from_matrix(rot_err_mat).as_rotvec(degrees=False) - self.state.pose_error[3:] = -rot_err_rotvex - - #wrench_err = self.state.external_wrench_base - self.state.desired_wrench + # 缓存常用计算 + arm_ori_quat = R.from_quat(self.state.arm_orientation) + arm_ori_mat = arm_ori_quat.as_matrix() + # 位置误差 + temp_pose_error = self.state.arm_position - self.state.desired_position + self.state.pose_error[:3] = arm_ori_mat.T @ temp_pose_error + # 姿态误差(四元数) + rot_err_quat = arm_ori_quat.inv() * R.from_quat(self.state.desired_orientation) + self.state.pose_error[3:] = -rot_err_quat.as_rotvec(degrees=False) + # 期望加速度 wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench - # if time.time() - self.laset_print_time > 1: - # print(f'wrench_err: {wrench_err} ||| external_wrench_tcp: {self.state.external_wrench_tcp} ||| desired_wrench: {self.state.desired_wrench}') - # self.state.arm_desired_acc = np.linalg.inv(self.M) @ (wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error) - self.state.arm_desired_acc = self.M_inv @ (wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error) - # if time.time() - self.laset_print_time > 5: - # print("@@@:",wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error) - self.clip_command(self.state.arm_desired_acc,"acc") - self.state.arm_desired_twist = self.state.arm_desired_acc * dt + self.state.arm_desired_twist - self.clip_command(self.state.arm_desired_twist,"vel") - delta_pose = self.state.arm_desired_twist * dt - - # delta_pose[:3] = self.pos_scale_factor * delta_pose[:3] - # delta_pose[3:] = self.rot_scale_factor * delta_pose[3:] - # if time.time() - self.laset_print_time > 5: - # print("delta_pose:",delta_pose) - - delta_pose[:3] = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_pose[:3] - - # if time.time() - self.laset_print_time > 5: - # print("tf_delta_pose:",delta_pose) - self.clip_command(delta_pose,"pose") - - # testlsy - delta_ori_mat = R.from_rotvec(delta_pose[3:]).as_matrix() - - # random_array = np.random.rand(3) # 生成长度为 3 的数组 - # delta_ori_mat = R.from_rotvec(random_array/100000).as_matrix() - - arm_ori_mat = delta_ori_mat @ R.from_quat(self.state.arm_orientation).as_matrix() - arm_ori_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_ori_mat - self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat() + KD_vel = self.D @ (self.state.arm_desired_twist - self.state.desired_twist) + KD_pose = self.K @ self.state.pose_error + self.state.arm_desired_acc = self.M_inv @ (wrench_err - KD_vel - KD_pose) + self.clip_command(self.state.arm_desired_acc, "acc") + ## 更新速度和位姿 + self.state.arm_desired_twist += self.state.arm_desired_acc * dt + self.clip_command(self.state.arm_desired_twist, "vel") + # 计算位姿变化 + delta_pose = np.concatenate([ + arm_ori_mat @ (self.state.arm_desired_twist[:3] * dt), + self.state.arm_desired_twist[3:] * dt + ]) + self.clip_command(delta_pose, "pose") + # 更新四元数 + delta_ori_quat = R.from_rotvec(delta_pose[3:]).as_quat() + arm_ori_quat_new = arm_ori_quat * R.from_quat(delta_ori_quat) + self.state.arm_orientation_command = arm_ori_quat_new.as_quat() # 归一化四元数 self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command) - + # 更新位置 self.state.arm_position_command = self.state.arm_position + delta_pose[:3] - # if time.time() - self.laset_print_time > 2: - # print("-------------admittance_1-------------------------------") - # print("arm_position:",self.state.arm_position) - # print("desired_position:",self.state.desired_position) - # print(f'wrench_err: {wrench_err} ||| external_wrench_tcp: {self.state.external_wrench_tcp} ||| desired_wrench: {self.state.desired_wrench}') - # print("self.state.arm_desired_acc:",self.state.arm_desired_acc) - # print("arm_orientation",R.from_quat(self.state.arm_orientation).as_euler('xyz',degrees=True)) - # print("desired_orientation",R.from_quat(self.state.desired_orientation).as_euler('xyz',degrees=True)) - # print("arm_position_command",self.state.arm_position_command) - # print("arm_orientation_command",R.from_quat(self.state.arm_orientation_command).as_euler('xyz',degrees=True)) - # print("delta_pose:",delta_pose) - # print("delta_ori_mat",delta_ori_mat) - # self.laset_print_time = time.time() + + + # # 计算误差 位置直接作差，姿态误差以旋转向量表示 + # temp_pose_error = self.state.arm_position - self.state.desired_position + # if self.state.desired_orientation.dot(self.state.arm_orientation) < 0: + # self.state.arm_orientation = -self.state.arm_orientation + # self.state.pose_error[:3] = R.from_quat(self.state.arm_orientation).as_matrix().T @ temp_pose_error + # rot_err_mat = R.from_quat(self.state.arm_orientation).as_matrix().T @ R.from_quat(self.state.desired_orientation).as_matrix() + # rot_err_rotvex = R.from_matrix(rot_err_mat).as_rotvec(degrees=False) + # self.state.pose_error[3:] = -rot_err_rotvex + # wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench + # self.state.arm_desired_acc = self.M_inv @ (wrench_err - self.D @ (self.state.arm_desired_twist -self.state.desired_twist) - self.K @ self.state.pose_error) + # self.clip_command(self.state.arm_desired_acc,"acc") + # self.state.arm_desired_twist = self.state.arm_desired_acc * dt + self.state.arm_desired_twist + # self.clip_command(self.state.arm_desired_twist,"vel") + # delta_pose = self.state.arm_desired_twist * dt + # delta_pose[:3] = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_pose[:3] + # self.clip_command(delta_pose,"pose") + # # testlsy + # delta_ori_mat = R.from_rotvec(delta_pose[3:]).as_matrix() + # ## 关闭姿态转动 + # # random_array = np.random.rand(3) # 生成长度为 3 的数组 + # # delta_ori_mat = R.from_rotvec(random_array/100000).as_matrix() + # arm_ori_mat = delta_ori_mat @ R.from_quat(self.state.arm_orientation).as_matrix() + # arm_ori_mat = R.from_quat(self.state.arm_orientation).as_matrix() @ delta_ori_mat + # self.state.arm_orientation_command = R.from_matrix(arm_ori_mat).as_quat() + # # 归一化四元数 + # self.state.arm_orientation_command /= np.linalg.norm(self.state.arm_orientation_command) + # self.state.arm_position_command = self.state.arm_position + delta_pose[:3] + # # if time.time() - self.laset_print_time > 2: + # # print("-------------admittance_1-------------------------------") + # # print("arm_position:",self.state.arm_position) + # # print("desired_position:",self.state.desired_position) + # # print(f'wrench_err: {wrench_err} ||| external_wrench_tcp: {self.state.external_wrench_tcp} ||| desired_wrench: {self.state.desired_wrench}') + # # print("self.state.arm_desired_acc:",self.state.arm_desired_acc) + # # print("arm_orientation",R.from_quat(self.state.arm_orientation).as_euler('xyz',degrees=True)) + # # print("desired_orientation",R.from_quat(self.state.desired_orientation).as_euler('xyz',degrees=True)) + # # print("arm_position_command",self.state.arm_position_command) + # # print("arm_orientation_command",R.from_quat(self.state.arm_orientation_command).as_euler('xyz',degrees=True)) + # # print("delta_pose:",delta_pose) + # # print("delta_ori_mat",delta_ori_mat) + # # self.laset_print_time = time.time() diff --git a/logs/MassageRobot_nova5_test.log b/logs/MassageRobot_nova5_test.log index e634b6b..4433e74 100644 --- a/logs/MassageRobot_nova5_test.log +++ b/logs/MassageRobot_nova5_test.log @@ -1,1264 +1,168 @@ -2025-05-21 15:38:51,180 - 测试日志 - INFO - log.py:106 - 切换到finger_head按摩头 -2025-05-21 15:38:54,693 - 测试日志 - INFO - log.py:106 - 机械臂测量线程启动 -2025-05-21 15:38:54,693 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999995 -0.13500023 0.34428417] -2025-05-21 15:38:54,695 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999531e+02 -4.50117128e-04 -9.00005367e+01] -2025-05-21 15:38:54,698 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999992 -0.13500023 0.34433902] -2025-05-21 15:38:54,699 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -4.79370603e-07 -1.57080019e+00] -2025-05-21 15:38:54,701 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999992 -0.13500023 0.34423885] -2025-05-21 15:38:54,702 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999598e+02 -3.85562286e-04 -9.00006915e+01] -2025-05-21 15:38:54,706 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999966 -0.13500023 0.34433899] -2025-05-21 15:38:54,708 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -9.58739618e-07 -1.57079966e+00] -2025-05-21 15:38:54,724 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999972 -0.13500023 0.34420165] -2025-05-21 15:38:54,726 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999989e+02 -4.02003154e-04 -9.00003638e+01] -2025-05-21 15:38:54,728 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999977 -0.13500023 0.34433902] -2025-05-21 15:38:54,729 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -9.58737967e-07 -1.57080019e+00] -2025-05-21 15:38:54,741 - 测试日志 - INFO - log.py:106 - position command: [ 0.2500003 -0.13500001 0.3441722 ] -2025-05-21 15:38:54,745 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999488e+02 -4.31166036e-04 -9.00006636e+01] -2025-05-21 15:38:54,747 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.13500023 0.34433908] -2025-05-21 15:38:54,749 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -4.79367142e-07 -1.57080059e+00] -2025-05-21 15:38:54,776 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000029 -0.13500002 0.3441475 ] -2025-05-21 15:38:54,780 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999475e+02 -1.25114521e-04 -9.00003792e+01] -2025-05-21 15:38:54,796 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999994 -0.13500035 0.34433914] -2025-05-21 15:38:54,797 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -1.43810701e-06 -1.57080019e+00] -2025-05-21 15:38:54,804 - 测试日志 - INFO - log.py:106 - position command: [ 0.2499999 -0.13499964 0.34412657] -2025-05-21 15:38:54,805 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999987e+02 -5.43937643e-04 -9.00004777e+01] -2025-05-21 15:38:54,806 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999991 -0.13499963 0.34433868] -2025-05-21 15:38:54,807 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -3.21676019e-12 -1.57079819e+00] -2025-05-21 15:38:54,821 - 测试日志 - INFO - log.py:106 - position command: [ 0.2499999 -0.13499966 0.34410693] -2025-05-21 15:38:54,828 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999669e+02 -6.44596275e-05 -9.00002518e+01] -2025-05-21 15:38:54,831 - 测试日志 - INFO - log.py:106 - position current:[ 0.2500005 -0.135 0.3443399] -2025-05-21 15:38:54,840 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 3.21698224e-12 -1.57079966e+00] -2025-05-21 15:38:54,852 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999996 -0.13499979 0.34408917] -2025-05-21 15:38:54,853 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999804e+02 -4.75766496e-04 -9.00001930e+01] -2025-05-21 15:38:54,854 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000002 -0.135 0.34433914] -2025-05-21 15:38:54,855 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -4.79368983e-07 -1.57079872e+00] -2025-05-21 15:38:54,868 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000017 -0.13500016 0.34407588] -2025-05-21 15:38:54,870 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999378e+02 -5.23441770e-05 -9.00002804e+01] -2025-05-21 15:38:54,880 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000008 -0.13499988 0.34433914] -2025-05-21 15:38:54,882 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -4.79368984e-07 -1.57079819e+00] -2025-05-21 15:38:54,884 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000033 -0.13500049 0.3440663 ] -2025-05-21 15:38:54,885 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999812e+02 -4.33889523e-04 -9.00003056e+01] -2025-05-21 15:38:54,887 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000035 -0.13500047 0.34433984] -2025-05-21 15:38:54,889 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159079e+00 -1.43809965e-06 -1.57080059e+00] -2025-05-21 15:38:54,913 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999954 -0.13500014 0.34405854] -2025-05-21 15:38:54,921 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999909e+02 -2.85497650e-04 -9.00003310e+01] -2025-05-21 15:38:54,922 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000008 -0.13499963 0.34433881] -2025-05-21 15:38:54,923 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -2.75734990e-12 -1.57079779e+00] -2025-05-21 15:38:54,928 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000023 -0.13500003 0.34405356] -2025-05-21 15:38:54,929 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999729e+02 -1.13830894e-04 -9.00005797e+01] -2025-05-21 15:38:54,930 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999977 -0.135 0.3443392 ] -2025-05-21 15:38:54,944 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265 0. -1.57079966] -2025-05-21 15:38:54,949 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999959 -0.13500015 0.34404851] -2025-05-21 15:38:54,953 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999690e+02 -1.91780884e-04 -9.00004614e+01] -2025-05-21 15:38:54,957 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000011 -0.135 0.34433908] -2025-05-21 15:38:54,959 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265 0. -1.57079966] -2025-05-21 15:38:54,966 - 测试日志 - INFO - log.py:106 - position command: [ 0.2500001 -0.13500003 0.34404578] -2025-05-21 15:38:54,972 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999797e+02 -2.77849887e-04 -9.00002433e+01] -2025-05-21 15:38:54,974 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.13500023 0.34433926] -2025-05-21 15:38:54,980 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -4.79367142e-07 -1.57080059e+00] -2025-05-21 15:38:54,984 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000028 -0.13500013 0.3440438 ] -2025-05-21 15:38:54,986 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999454e+02 -2.34525809e-04 -9.00007132e+01] -2025-05-21 15:38:54,987 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999995 -0.135 0.34433926] -2025-05-21 15:38:55,002 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -4.79367396e-07 -1.57079926e+00] -2025-05-21 15:38:55,004 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000006 -0.13499991 0.34404186] -2025-05-21 15:38:55,005 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999740e+02 -3.92961849e-04 -9.00002362e+01] -2025-05-21 15:38:55,006 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999994 -0.13500012 0.34433914] -2025-05-21 15:38:55,007 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -4.79368984e-07 -1.57079966e+00] -2025-05-21 15:38:55,032 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999997 -0.13500049 0.34404083] -2025-05-21 15:38:55,033 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999478e+02 -1.46487771e-04 -9.00007290e+01] -2025-05-21 15:38:55,040 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999977 -0.135 0.34433887] -2025-05-21 15:38:55,043 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -4.79372222e-07 -1.57079926e+00] -2025-05-21 15:38:55,049 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000006 -0.13500002 0.34403961] -2025-05-21 15:38:55,050 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999706e+02 -6.06408321e-04 -9.00006270e+01] -2025-05-21 15:38:55,051 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999995 -0.13499963 0.3443389 ] -2025-05-21 15:38:55,052 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159132e+00 -3.44679840e-12 -1.57079726e+00] -2025-05-21 15:38:55,071 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000022 -0.13499969 0.34403875] -2025-05-21 15:38:55,074 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999714e+02 -2.00425376e-04 -9.00003465e+01] -2025-05-21 15:38:55,080 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.135 0.34433902] -2025-05-21 15:38:55,085 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.14863674e-12 -1.57079872e+00] -2025-05-21 15:38:55,094 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000022 -0.1349997 0.3440381 ] -2025-05-21 15:38:55,097 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999781e+02 -5.31536942e-04 -9.00004553e+01] -2025-05-21 15:38:55,100 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000017 -0.13499988 0.34433908] -2025-05-21 15:38:55,105 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265 0. -1.57079926] -2025-05-21 15:38:55,111 - 测试日志 - INFO - log.py:106 - 机械臂控制线程启动 -2025-05-21 15:38:55,111 - 测试日志 - INFO - log.py:106 - MassageRobot启动 -2025-05-21 15:41:29,307 - 测试日志 - INFO - log.py:106 - 切换到finger_head按摩头 -2025-05-21 15:41:32,818 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000003 -0.13500047 0.34428353] -2025-05-21 15:41:32,820 - 测试日志 - INFO - log.py:106 - 机械臂测量线程启动 -2025-05-21 15:41:32,820 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999514e+02 -4.36291462e-04 -9.00004551e+01] -2025-05-21 15:41:32,822 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000044 -0.13500035 0.34433926] -2025-05-21 15:41:32,824 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -1.43810701e-06 -1.57079686e+00] -2025-05-21 15:41:32,826 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000043 -0.13499033 0.34423907] -2025-05-21 15:41:32,828 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999785e+02 -1.50884930e-04 -9.00003968e+01] -2025-05-21 15:41:32,829 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000044 -0.13500035 0.34433926] -2025-05-21 15:41:32,830 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -1.43810701e-06 -1.57079686e+00] -2025-05-21 15:41:32,846 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000042 -0.13498163 0.34420217] -2025-05-21 15:41:32,847 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999571e+02 -1.33410618e-04 -9.00003322e+01] -2025-05-21 15:41:32,848 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999963 -0.1350007 0.34433893] -2025-05-21 15:41:32,850 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.43810701e-06 -1.57079779e+00] -2025-05-21 15:41:32,866 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999998 -0.13497825 0.34417174] -2025-05-21 15:41:32,867 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999523e+02 -4.44760017e-04 -9.00001619e+01] -2025-05-21 15:41:32,869 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000011 -0.1350006 0.34433978] -2025-05-21 15:41:32,871 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159026e+00 -4.79368983e-07 -1.57079819e+00] -2025-05-21 15:41:32,883 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000009 -0.13497469 0.34414779] -2025-05-21 15:41:32,885 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999717e+02 -5.65482109e-05 -9.00004179e+01] -2025-05-21 15:41:32,887 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999991 -0.13500035 0.34433856] -2025-05-21 15:41:32,889 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159079e+00 -9.58739808e-07 -1.57079779e+00] -2025-05-21 15:41:32,902 - 测试日志 - INFO - log.py:106 - position command: [ 0.2499999 -0.13497354 0.34412638] -2025-05-21 15:41:32,905 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999831e+02 -3.41982675e-04 -9.00004848e+01] -2025-05-21 15:41:32,913 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000006 -0.13500023 0.34433936] -2025-05-21 15:41:32,916 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -9.58737777e-07 -1.57079686e+00] -2025-05-21 15:41:32,925 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999974 -0.13497126 0.34411007] -2025-05-21 15:41:32,927 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999926e+02 -3.50854046e-04 -9.00000850e+01] -2025-05-21 15:41:32,930 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000023 -0.13500012 0.34433929] -2025-05-21 15:41:32,931 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -4.79368761e-07 -1.57079686e+00] -2025-05-21 15:41:32,949 - 测试日志 - INFO - log.py:106 - position command: [ 0.2500003 -0.13497046 0.34409638] -2025-05-21 15:41:32,951 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999904e+02 -5.34757889e-04 -9.00002543e+01] -2025-05-21 15:41:32,953 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000005 -0.13500035 0.34433923] -2025-05-21 15:41:32,954 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -9.58737967e-07 -1.57079686e+00] -2025-05-21 15:41:32,969 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000023 -0.13496968 0.34408659] -2025-05-21 15:41:32,970 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999366e+02 -5.33738918e-05 -9.00002417e+01] -2025-05-21 15:41:32,996 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000002 -0.13500047 0.34433981] -2025-05-21 15:41:33,000 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159079e+00 -1.43810511e-06 -1.57079779e+00] -2025-05-21 15:41:33,005 - 测试日志 - INFO - log.py:106 - position command: [ 0.25 -0.13496832 0.34407742] -2025-05-21 15:41:33,009 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999655e+02 -5.07907069e-04 -9.00001090e+01] -2025-05-21 15:41:33,013 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000005 -0.13500012 0.3443389 ] -2025-05-21 15:41:33,015 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -4.79369904e-07 -1.57079726e+00] -2025-05-21 15:41:33,021 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000032 -0.13496704 0.34406955] -2025-05-21 15:41:33,022 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999597e+02 -5.43679625e-04 -9.00002741e+01] -2025-05-21 15:41:33,024 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000015 -0.13500023 0.34433899] -2025-05-21 15:41:33,026 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -4.79369460e-07 -1.57079726e+00] -2025-05-21 15:41:33,039 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999999 -0.13496806 0.34406316] -2025-05-21 15:41:33,040 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999921e+02 -3.21349605e-04 -9.00002654e+01] -2025-05-21 15:41:33,042 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999994 -0.13500047 0.34433929] -2025-05-21 15:41:33,045 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 -1.43810651e-06 -1.57079726e+00] -2025-05-21 15:41:33,060 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999993 -0.13497049 0.34405848] -2025-05-21 15:41:33,061 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999691e+02 -4.33099953e-04 -9.00004089e+01] -2025-05-21 15:41:33,066 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000005 -0.13500047 0.34433942] -2025-05-21 15:41:33,069 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 -9.58736634e-07 -1.57079872e+00] -2025-05-21 15:41:33,080 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999999 -0.13497303 0.34405415] -2025-05-21 15:41:33,081 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999994e+02 -4.33111404e-04 -9.00004383e+01] -2025-05-21 15:41:33,083 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999998 -0.13500047 0.3443389 ] -2025-05-21 15:41:33,084 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -1.43810790e-06 -1.57079779e+00] -2025-05-21 15:41:33,102 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999998 -0.13497626 0.34405072] -2025-05-21 15:41:33,105 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999798e+02 -3.14187246e-04 -9.00003675e+01] -2025-05-21 15:41:33,118 - 测试日志 - INFO - log.py:106 - position current:[ 0.2499998 -0.13500035 0.34433835] -2025-05-21 15:41:33,121 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159026e+00 -9.58740316e-07 -1.57079779e+00] -2025-05-21 15:41:33,132 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999989 -0.13497615 0.34404757] -2025-05-21 15:41:33,134 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999568e+02 -5.52178897e-04 -9.00002003e+01] -2025-05-21 15:41:33,136 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.13500035 0.34433936] -2025-05-21 15:41:33,138 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -9.58737523e-07 -1.57079779e+00] -2025-05-21 15:41:33,147 - 测试日志 - INFO - log.py:106 - position command: [ 0.2499999 -0.13497436 0.34404616] -2025-05-21 15:41:33,155 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999640e+02 -1.89991748e-04 -9.00002335e+01] -2025-05-21 15:41:33,160 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.13500035 0.34433884] -2025-05-21 15:41:33,163 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.43810739e-06 -1.57079726e+00] -2025-05-21 15:41:33,166 - 测试日志 - INFO - log.py:106 - position command: [ 0.25 -0.13497282 0.34404392] -2025-05-21 15:41:33,170 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999687e+02 -2.03209759e-04 -9.00005518e+01] -2025-05-21 15:41:33,172 - 测试日志 - INFO - log.py:106 - position current:[ 0.2499996 -0.13500047 0.34433914] -2025-05-21 15:41:33,178 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -9.58737522e-07 -1.57079819e+00] -2025-05-21 15:41:33,186 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999966 -0.13497349 0.34404262] -2025-05-21 15:41:33,188 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999809e+02 -3.84174118e-04 -9.00006367e+01] -2025-05-21 15:41:33,189 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000029 -0.13500023 0.34433929] -2025-05-21 15:41:33,190 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.43810701e-06 -1.57079579e+00] -2025-05-21 15:41:33,205 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000022 -0.13497483 0.3440417 ] -2025-05-21 15:41:33,215 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999699e+02 -2.78844792e-04 -9.00001544e+01] -2025-05-21 15:41:33,219 - 测试日志 - INFO - log.py:106 - position current:[ 0.2500002 -0.13500023 0.34433887] -2025-05-21 15:41:33,230 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159132e+00 -4.79368983e-07 -1.57079686e+00] -2025-05-21 15:41:33,231 - 测试日志 - INFO - log.py:106 - 机械臂控制线程启动 -2025-05-21 15:41:33,235 - 测试日志 - INFO - log.py:106 - MassageRobot启动 -2025-05-21 15:43:48,717 - 测试日志 - INFO - log.py:106 - 切换到finger_head按摩头 -2025-05-21 15:43:52,227 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999983 -0.13498364 0.34428417] -2025-05-21 15:43:52,227 - 测试日志 - INFO - log.py:106 - 机械臂测量线程启动 -2025-05-21 15:43:52,229 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999856e+02 3.32994220e-03 -9.00001526e+01] -2025-05-21 15:43:52,230 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.13498364 0.34433926] -2025-05-21 15:43:52,232 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 6.80703991e-05 -1.57079872e+00] -2025-05-21 15:43:52,233 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000011 -0.13498381 0.34423932] -2025-05-21 15:43:52,234 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999625e+02 3.77403298e-03 -9.00004862e+01] -2025-05-21 15:43:52,235 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000012 -0.13498387 0.34433951] -2025-05-21 15:43:52,236 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 6.66322937e-05 -1.57079872e+00] -2025-05-21 15:43:52,252 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999972 -0.13498398 0.34420226] -2025-05-21 15:43:52,253 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999688e+02 3.47278350e-03 -9.00002621e+01] -2025-05-21 15:43:52,255 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000027 -0.13498387 0.34433899] -2025-05-21 15:43:52,255 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 6.61529199e-05 -1.57079779e+00] -2025-05-21 15:43:52,270 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000004 -0.13498428 0.34417217] -2025-05-21 15:43:52,270 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999900e+02 3.22378552e-03 -9.00006058e+01] -2025-05-21 15:43:52,271 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000015 -0.13498422 0.34433884] -2025-05-21 15:43:52,271 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 6.56735500e-05 -1.57079926e+00] -2025-05-21 15:43:52,288 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999971 -0.13498423 0.34414694] -2025-05-21 15:43:52,289 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999784e+02 3.16487779e-03 -9.00007029e+01] -2025-05-21 15:43:52,289 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.13498457 0.34433942] -2025-05-21 15:43:52,290 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 6.47148186e-05 -1.57079926e+00] -2025-05-21 15:43:52,307 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999983 -0.13498419 0.34412714] -2025-05-21 15:43:52,308 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999687e+02 3.51888567e-03 -9.00004964e+01] -2025-05-21 15:43:52,309 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.13498445 0.34433926] -2025-05-21 15:43:52,311 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 6.47148145e-05 -1.57079872e+00] -2025-05-21 15:43:52,325 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999995 -0.13498416 0.34411094] -2025-05-21 15:43:52,326 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999720e+02 3.30575828e-03 -9.00001902e+01] -2025-05-21 15:43:52,326 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999995 -0.13498445 0.3443396 ] -2025-05-21 15:43:52,327 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 6.42354487e-05 -1.57079872e+00] -2025-05-21 15:43:52,343 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999956 -0.13498448 0.34409734] -2025-05-21 15:43:52,344 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999681e+02 3.58203902e-03 -9.00005903e+01] -2025-05-21 15:43:52,344 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999956 -0.1349848 0.34433954] -2025-05-21 15:43:52,345 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 6.32767091e-05 -1.57079926e+00] -2025-05-21 15:43:52,361 - 测试日志 - INFO - log.py:106 - position command: [ 0.2499999 -0.13498423 0.34408601] -2025-05-21 15:43:52,363 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999501e+02 3.38153780e-03 -9.00005836e+01] -2025-05-21 15:43:52,363 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000011 -0.1349848 0.34433939] -2025-05-21 15:43:52,364 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 6.27973393e-05 -1.57079819e+00] -2025-05-21 15:43:52,380 - 测试日志 - INFO - log.py:106 - position command: [ 0.2500001 -0.13498455 0.34407725] -2025-05-21 15:43:52,382 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999813e+02 3.17534464e-03 -9.00007084e+01] -2025-05-21 15:43:52,383 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000011 -0.13498492 0.34433957] -2025-05-21 15:43:52,385 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 6.27973393e-05 -1.57079872e+00] -2025-05-21 15:43:52,399 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000017 -0.13498485 0.34406984] -2025-05-21 15:43:52,400 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999936e+02 3.34749959e-03 -9.00001786e+01] -2025-05-21 15:43:52,403 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000018 -0.13498526 0.34433957] -2025-05-21 15:43:52,404 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 6.08798641e-05 -1.57079819e+00] -2025-05-21 15:43:52,417 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000013 -0.13498507 0.34406321] -2025-05-21 15:43:52,418 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999850e+02 3.14131221e-03 -9.00003222e+01] -2025-05-21 15:43:52,420 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000005 -0.13498538 0.3443389 ] -2025-05-21 15:43:52,421 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 6.08798641e-05 -1.57079926e+00] -2025-05-21 15:43:52,435 - 测试日志 - INFO - log.py:106 - position command: [ 0.2499999 -0.13498514 0.34405844] -2025-05-21 15:43:52,436 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999429e+02 3.00942355e-03 -9.00007527e+01] -2025-05-21 15:43:52,437 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000005 -0.13498561 0.34433923] -2025-05-21 15:43:52,439 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 5.99211245e-05 -1.57079872e+00] -2025-05-21 15:43:52,454 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000004 -0.13498512 0.34405457] -2025-05-21 15:43:52,455 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999611e+02 3.24209473e-03 -9.00002639e+01] -2025-05-21 15:43:52,456 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000006 -0.13498561 0.34433936] -2025-05-21 15:43:52,457 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 5.94417547e-05 -1.57079872e+00] -2025-05-21 15:43:52,473 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999992 -0.1349851 0.34405119] -2025-05-21 15:43:52,474 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999710e+02 3.23052108e-03 -9.00006916e+01] -2025-05-21 15:43:52,476 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999989 -0.13498584 0.34433908] -2025-05-21 15:43:52,477 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 5.84830192e-05 -1.57079819e+00] -2025-05-21 15:43:52,490 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999982 -0.13498542 0.34404859] -2025-05-21 15:43:52,493 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999582e+02 3.32614032e-03 -9.00004215e+01] -2025-05-21 15:43:52,495 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.13498596 0.34433887] -2025-05-21 15:43:52,496 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 5.80036494e-05 -1.57079872e+00] -2025-05-21 15:43:52,509 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999982 -0.13498551 0.34404604] -2025-05-21 15:43:52,511 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999547e+02 2.80161022e-03 -9.00004232e+01] -2025-05-21 15:43:52,513 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000023 -0.13498607 0.34433945] -2025-05-21 15:43:52,515 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 5.75242795e-05 -1.57079872e+00] -2025-05-21 15:43:52,527 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999972 -0.13498561 0.34404447] -2025-05-21 15:43:52,528 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999429e+02 3.14628048e-03 -9.00006921e+01] -2025-05-21 15:43:52,529 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999948 -0.13498631 0.34433893] -2025-05-21 15:43:52,530 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 5.75242795e-05 -1.57079926e+00] -2025-05-21 15:43:52,545 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999999 -0.13498285 0.34404316] -2025-05-21 15:43:52,547 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999680e+02 2.87319968e-03 -9.00007308e+01] -2025-05-21 15:43:52,548 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.13498643 0.34433951] -2025-05-21 15:43:52,549 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 5.70449138e-05 -1.57079966e+00] -2025-05-21 15:43:52,564 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999991 -0.13498189 0.34404161] -2025-05-21 15:43:52,565 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999808e+02 2.77687391e-03 -9.00005849e+01] -2025-05-21 15:43:52,568 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999991 -0.13498642 0.34434003] -2025-05-21 15:43:52,569 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14158972e+00 5.65655481e-05 -1.57079926e+00] -2025-05-21 15:43:52,583 - 测试日志 - INFO - log.py:106 - 机械臂控制线程启动 -2025-05-21 15:43:52,585 - 测试日志 - INFO - log.py:106 - MassageRobot启动 -2025-05-21 15:44:04,123 - 测试日志 - INFO - log.py:106 - 切换到finger_head按摩头 -2025-05-21 15:44:07,638 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000021 -0.13503959 0.34428423] -2025-05-21 15:44:07,639 - 测试日志 - INFO - log.py:106 - 机械臂测量线程启动 -2025-05-21 15:44:07,640 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999474e+02 -9.42900818e-03 -9.00000105e+01] -2025-05-21 15:44:07,641 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999992 -0.13503923 0.34433917] -2025-05-21 15:44:07,643 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.61547345e-04 -1.57079633e+00] -2025-05-21 15:44:07,646 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999993 -0.13503937 0.344239 ] -2025-05-21 15:44:07,647 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999504e+02 -9.76494460e-03 -9.00001775e+01] -2025-05-21 15:44:07,649 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999992 -0.13503923 0.34433917] -2025-05-21 15:44:07,650 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.61547345e-04 -1.57079633e+00] -2025-05-21 15:44:07,665 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999994 -0.13503902 0.34420198] -2025-05-21 15:44:07,666 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999628e+02 -9.21376681e-03 -9.00003704e+01] -2025-05-21 15:44:07,667 - 测试日志 - INFO - log.py:106 - position current:[ 0.2499995 -0.13503853 0.34433923] -2025-05-21 15:44:07,669 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.58671134e-04 -1.57079579e+00] -2025-05-21 15:44:07,682 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000003 -0.13503857 0.34417243] -2025-05-21 15:44:07,684 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999819e+02 -9.39420466e-03 -9.00001859e+01] -2025-05-21 15:44:07,686 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000002 -0.13503818 0.34433966] -2025-05-21 15:44:07,687 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159079e+00 -1.57233037e-04 -1.57079633e+00] -2025-05-21 15:44:07,701 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000003 -0.13503867 0.34414773] -2025-05-21 15:44:07,703 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999611e+02 -9.35521916e-03 -9.00000452e+01] -2025-05-21 15:44:07,704 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.13503807 0.34433969] -2025-05-21 15:44:07,705 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 -1.56274290e-04 -1.57079686e+00] -2025-05-21 15:44:07,719 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999992 -0.13503796 0.34412714] -2025-05-21 15:44:07,721 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999519e+02 -9.07322490e-03 -9.00000820e+01] -2025-05-21 15:44:07,722 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999992 -0.1350376 0.34433902] -2025-05-21 15:44:07,723 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.54836192e-04 -1.57079633e+00] -2025-05-21 15:44:07,737 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000015 -0.1350348 0.34411135] -2025-05-21 15:44:07,739 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999821e+02 -9.34678806e-03 -9.00004629e+01] -2025-05-21 15:44:07,740 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999982 -0.13503691 0.34433896] -2025-05-21 15:44:07,741 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -1.51959965e-04 -1.57079579e+00] -2025-05-21 15:44:07,756 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999985 -0.13503285 0.34409678] -2025-05-21 15:44:07,757 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999950e+02 -9.15551380e-03 -9.00005206e+01] -2025-05-21 15:44:07,758 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999998 -0.13503644 0.34433875] -2025-05-21 15:44:07,760 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -1.50042502e-04 -1.57079579e+00] -2025-05-21 15:44:07,774 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000002 -0.13503169 0.34408552] -2025-05-21 15:44:07,775 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999967e+02 -8.86227011e-03 -9.00002479e+01] -2025-05-21 15:44:07,776 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000011 -0.13503609 0.34433957] -2025-05-21 15:44:07,777 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 -1.48604389e-04 -1.57079686e+00] -2025-05-21 15:44:07,792 - 测试日志 - INFO - log.py:106 - position command: [ 0.2499999 -0.13503102 0.34407657] -2025-05-21 15:44:07,793 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999719e+02 -8.55934293e-03 -9.00001735e+01] -2025-05-21 15:44:07,795 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999977 -0.13503563 0.34433893] -2025-05-21 15:44:07,796 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.46686910e-04 -1.57079579e+00] -2025-05-21 15:44:07,810 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000002 -0.13503092 0.34406909] -2025-05-21 15:44:07,812 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999732e+02 -8.65197896e-03 -9.00004110e+01] -2025-05-21 15:44:07,813 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000008 -0.13503505 0.34433963] -2025-05-21 15:44:07,814 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159079e+00 -1.44769430e-04 -1.57079579e+00] -2025-05-21 15:44:07,829 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000011 -0.13503092 0.3440639 ] -2025-05-21 15:44:07,830 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999737e+02 -8.37332163e-03 -9.00004046e+01] -2025-05-21 15:44:07,831 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000017 -0.1350347 0.34433896] -2025-05-21 15:44:07,832 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.43331333e-04 -1.57079579e+00] -2025-05-21 15:44:07,846 - 测试日志 - INFO - log.py:106 - position command: [ 0.2500002 -0.13503112 0.34405827] -2025-05-21 15:44:07,847 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999541e+02 -8.29450128e-03 -9.00003121e+01] -2025-05-21 15:44:07,848 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000018 -0.13503447 0.34433923] -2025-05-21 15:44:07,849 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 -1.42372585e-04 -1.57079579e+00] -2025-05-21 15:44:07,865 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000021 -0.13503148 0.34405449] -2025-05-21 15:44:07,866 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999533e+02 -8.39908922e-03 -9.00000389e+01] -2025-05-21 15:44:07,867 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000008 -0.13503435 0.3443385 ] -2025-05-21 15:44:07,868 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159079e+00 -1.41413854e-04 -1.57079579e+00] -2025-05-21 15:44:07,883 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999992 -0.13503182 0.34405135] -2025-05-21 15:44:07,884 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999869e+02 -8.32430638e-03 -9.00005710e+01] -2025-05-21 15:44:07,885 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999977 -0.13503389 0.34433905] -2025-05-21 15:44:07,886 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 -1.39975740e-04 -1.57079539e+00] -2025-05-21 15:44:07,902 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999981 -0.13502904 0.34404815] -2025-05-21 15:44:07,903 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999709e+02 -8.29396143e-03 -9.00002807e+01] -2025-05-21 15:44:07,904 - 测试日志 - INFO - log.py:106 - position current:[ 0.2499998 -0.13503354 0.34433966] -2025-05-21 15:44:07,905 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 -1.38537643e-04 -1.57079539e+00] -2025-05-21 15:44:07,920 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999985 -0.13502756 0.34404665] -2025-05-21 15:44:07,921 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999527e+02 -8.43792690e-03 -9.00000395e+01] -2025-05-21 15:44:07,922 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000011 -0.13503307 0.34433923] -2025-05-21 15:44:07,923 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -1.36620164e-04 -1.57079633e+00] -2025-05-21 15:44:07,938 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000015 -0.1350268 0.3440444 ] -2025-05-21 15:44:07,939 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999937e+02 -7.99236348e-03 -9.00003290e+01] -2025-05-21 15:44:07,940 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999989 -0.13503261 0.34433942] -2025-05-21 15:44:07,941 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.34702685e-04 -1.57079579e+00] -2025-05-21 15:44:07,956 - 测试日志 - INFO - log.py:106 - position command: [ 0.2500004 -0.13502646 0.34404348] -2025-05-21 15:44:07,957 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999616e+02 -8.21579422e-03 -9.00000057e+01] -2025-05-21 15:44:07,959 - 测试日志 - INFO - log.py:106 - position current:[ 0.2499996 -0.1350325 0.34433914] -2025-05-21 15:44:07,960 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.33743953e-04 -1.57079633e+00] -2025-05-21 15:44:07,974 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999975 -0.135027 0.34404165] -2025-05-21 15:44:07,980 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999705e+02 -8.12926368e-03 -9.00000579e+01] -2025-05-21 15:44:07,982 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999982 -0.13503226 0.34433881] -2025-05-21 15:44:07,984 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -1.32785214e-04 -1.57079579e+00] -2025-05-21 15:44:07,993 - 测试日志 - INFO - log.py:106 - MassageRobot启动 -2025-05-21 15:44:07,993 - 测试日志 - INFO - log.py:106 - 机械臂控制线程启动 -2025-05-21 15:50:05,598 - 测试日志 - INFO - log.py:106 - 切换到finger_head按摩头 -2025-05-21 15:50:09,111 - 测试日志 - INFO - log.py:106 - 机械臂测量线程启动 -2025-05-21 15:50:09,113 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999945 -0.13502949 0.34428408] -2025-05-21 15:50:09,114 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999939e+02 -6.94885245e-03 -9.00000839e+01] -2025-05-21 15:50:09,116 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000047 -0.13502888 0.34433936] -2025-05-21 15:50:09,117 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 -1.20321615e-04 -1.57079633e+00] -2025-05-21 15:50:09,120 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000044 -0.135029 0.34423918] -2025-05-21 15:50:09,121 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999939e+02 -6.91358436e-03 -8.99999787e+01] -2025-05-21 15:50:09,122 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000005 -0.13502913 0.34433871] -2025-05-21 15:50:09,123 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159079e+00 -1.20800989e-04 -1.57079686e+00] -2025-05-21 15:50:09,137 - 测试日志 - INFO - log.py:106 - position command: [ 0.2499995 -0.13502945 0.3441964 ] -2025-05-21 15:50:09,139 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999970e+02 -6.88259253e-03 -9.00001057e+01] -2025-05-21 15:50:09,145 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000018 -0.13502901 0.34433975] -2025-05-21 15:50:09,147 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159079e+00 -1.19362875e-04 -1.57079779e+00] -2025-05-21 15:50:09,155 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000007 -0.13503174 0.34416513] -2025-05-21 15:50:09,156 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999994e+02 -6.73043380e-03 -9.00000622e+01] -2025-05-21 15:50:09,160 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000011 -0.13502866 0.34433923] -2025-05-21 15:50:09,162 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -1.18404144e-04 -1.57079779e+00] -2025-05-21 15:50:09,200 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000004 -0.1350333 0.34414096] -2025-05-21 15:50:09,208 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999814e+02 -6.66216712e-03 -9.00000762e+01] -2025-05-21 15:50:09,209 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999995 -0.13502773 0.34433942] -2025-05-21 15:50:09,210 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.15048559e-04 -1.57079686e+00] -2025-05-21 15:50:09,212 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000003 -0.13503712 0.34411775] -2025-05-21 15:50:09,213 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999800e+02 -6.53672102e-03 -9.00000793e+01] -2025-05-21 15:50:09,215 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999995 -0.13502773 0.34433942] -2025-05-21 15:50:09,216 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.15048559e-04 -1.57079686e+00] -2025-05-21 15:50:09,237 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000003 -0.13503945 0.34409774] -2025-05-21 15:50:09,239 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999984e+02 -6.22242171e-03 -9.00001052e+01] -2025-05-21 15:50:09,241 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999968 -0.13502727 0.34433926] -2025-05-21 15:50:09,245 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -1.13131080e-04 -1.57079633e+00] -2025-05-21 15:50:09,252 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000038 -0.13503942 0.34408265] -2025-05-21 15:50:09,254 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999866e+02 -6.08093180e-03 -9.00000422e+01] -2025-05-21 15:50:09,255 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999973 -0.1350275 0.34433914] -2025-05-21 15:50:09,259 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.13610454e-04 -1.57079686e+00] -2025-05-21 15:50:09,269 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999988 -0.13503913 0.34407104] -2025-05-21 15:50:09,270 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999852e+02 -6.04762511e-03 -9.00000062e+01] -2025-05-21 15:50:09,271 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000021 -0.13502751 0.34433917] -2025-05-21 15:50:09,273 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -1.13131080e-04 -1.57079779e+00] -2025-05-21 15:50:09,308 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000023 -0.13504747 0.34405394] -2025-05-21 15:50:09,321 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999823e+02 -5.57848689e-03 -8.99999831e+01] -2025-05-21 15:50:09,323 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000006 -0.13502669 0.3443392 ] -2025-05-21 15:50:09,325 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.10254869e-04 -1.57079726e+00] -2025-05-21 15:50:09,343 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999998 -0.13504697 0.34404324] -2025-05-21 15:50:09,344 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999886e+02 -5.47855109e-03 -8.99999889e+01] -2025-05-21 15:50:09,345 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999995 -0.13502646 0.34433908] -2025-05-21 15:50:09,346 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.09775503e-04 -1.57079726e+00] -2025-05-21 15:50:09,347 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999987 -0.1350455 0.34403383] -2025-05-21 15:50:09,348 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999877e+02 -5.40459135e-03 -8.99999708e+01] -2025-05-21 15:50:09,349 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999942 -0.13502658 0.34433826] -2025-05-21 15:50:09,350 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159079e+00 -1.09296130e-04 -1.57079779e+00] -2025-05-21 15:50:09,365 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999993 -0.13504363 0.34402547] -2025-05-21 15:50:09,365 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999895e+02 -5.32846477e-03 -8.99999520e+01] -2025-05-21 15:50:09,366 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000011 -0.13502599 0.3443389 ] -2025-05-21 15:50:09,367 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -1.07858024e-04 -1.57079686e+00] -2025-05-21 15:50:09,385 - 测试日志 - INFO - log.py:106 - position command: [ 0.2499999 -0.13504143 0.34402214] -2025-05-21 15:50:09,388 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999985e+02 -5.27231188e-03 -8.99999691e+01] -2025-05-21 15:50:09,399 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000017 -0.13502588 0.34433896] -2025-05-21 15:50:09,409 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.06899285e-04 -1.57079726e+00] -2025-05-21 15:50:09,414 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000014 -0.13504211 0.3440217 ] -2025-05-21 15:50:09,418 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999892e+02 -5.13014501e-03 -8.99999730e+01] -2025-05-21 15:50:09,423 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999988 -0.13502576 0.34433896] -2025-05-21 15:50:09,424 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -1.06419919e-04 -1.57079686e+00] -2025-05-21 15:50:09,433 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999979 -0.13504296 0.34402074] -2025-05-21 15:50:09,448 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999905e+02 -5.01750610e-03 -8.99999236e+01] -2025-05-21 15:50:09,450 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000012 -0.13502553 0.34433969] -2025-05-21 15:50:09,451 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 -1.05461179e-04 -1.57079686e+00] -2025-05-21 15:50:09,460 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999972 -0.13504174 0.3440162 ] -2025-05-21 15:50:09,464 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999932e+02 -4.93468283e-03 -8.99999740e+01] -2025-05-21 15:50:09,465 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.13502553 0.34433926] -2025-05-21 15:50:09,467 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.04981813e-04 -1.57079819e+00] -2025-05-21 15:50:09,473 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999973 -0.13504338 0.34401465] -2025-05-21 15:50:09,476 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999913e+02 -4.82967506e-03 -8.99999724e+01] -2025-05-21 15:50:09,480 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.13502507 0.34433936] -2025-05-21 15:50:09,485 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.03064334e-04 -1.57079819e+00] -2025-05-21 15:50:09,491 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999991 -0.13504309 0.34401547] -2025-05-21 15:50:09,493 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999764e+02 -4.62548294e-03 -8.99999883e+01] -2025-05-21 15:50:09,495 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999986 -0.13502483 0.34433932] -2025-05-21 15:50:09,499 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 -1.03064334e-04 -1.57079726e+00] -2025-05-21 15:50:09,511 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999978 -0.1350398 0.34400821] -2025-05-21 15:50:09,512 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999723e+02 -4.65566711e-03 -8.99999140e+01] -2025-05-21 15:50:09,515 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000031 -0.13502483 0.34433975] -2025-05-21 15:50:09,516 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159079e+00 -1.03064334e-04 -1.57079686e+00] -2025-05-21 15:50:09,532 - 测试日志 - INFO - log.py:106 - 机械臂控制线程启动 -2025-05-21 15:50:09,536 - 测试日志 - INFO - log.py:106 - MassageRobot启动 -2025-05-21 15:52:11,088 - 测试日志 - INFO - log.py:106 - 切换到finger_head按摩头 -2025-05-21 15:52:14,599 - 测试日志 - INFO - log.py:106 - 机械臂测量线程启动 -2025-05-21 15:52:14,600 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000029 -0.13500012 0.34428453] -2025-05-21 15:52:14,601 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999924e+02 -2.74657814e-05 -9.00000305e+01] -2025-05-21 15:52:14,604 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.13500024 0.34433942] -2025-05-21 15:52:14,607 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132 0. -1.57079726] -2025-05-21 15:52:14,609 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999985 -0.13499021 0.34423925] -2025-05-21 15:52:14,611 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999866e+02 -8.85188747e-04 -9.00000533e+01] -2025-05-21 15:52:14,646 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000021 -0.135 0.34433917] -2025-05-21 15:52:14,648 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 4.79368983e-07 -1.57079726e+00] -2025-05-21 15:52:14,649 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000023 -0.13498127 0.34420213] -2025-05-21 15:52:14,650 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999924e+02 -1.59435965e-03 -9.00000216e+01] -2025-05-21 15:52:14,658 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000012 -0.13499988 0.34433951] -2025-05-21 15:52:14,659 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212 0. -1.57079579] -2025-05-21 15:52:14,676 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999986 -0.13497668 0.34416922] -2025-05-21 15:52:14,677 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999956e+02 -2.02014988e-03 -8.99999302e+01] -2025-05-21 15:52:14,678 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000006 -0.135 0.34433936] -2025-05-21 15:52:14,680 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212 0. -1.57079633] -2025-05-21 15:52:14,703 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999988 -0.13497304 0.34414254] -2025-05-21 15:52:14,705 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999820e+02 -2.34605182e-03 -8.99999850e+01] -2025-05-21 15:52:14,705 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999995 -0.135 0.34433942] -2025-05-21 15:52:14,706 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -4.79368761e-07 -1.57079633e+00] -2025-05-21 15:52:14,724 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999999 -0.13496894 0.3441179 ] -2025-05-21 15:52:14,725 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999901e+02 -2.70437887e-03 -8.99999534e+01] -2025-05-21 15:52:14,726 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999977 -0.135 0.3443392 ] -2025-05-21 15:52:14,727 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265 0. -1.57079633] -2025-05-21 15:52:14,745 - 测试日志 - INFO - log.py:106 - position command: [ 0.25 -0.1349652 0.34409918] -2025-05-21 15:52:14,747 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999890e+02 -3.06381531e-03 -8.99998839e+01] -2025-05-21 15:52:14,748 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.135 0.34433908] -2025-05-21 15:52:14,752 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212 0. -1.57079633] -2025-05-21 15:52:14,764 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999976 -0.13496122 0.34408316] -2025-05-21 15:52:14,766 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999966e+02 -3.33631727e-03 -8.99999365e+01] -2025-05-21 15:52:14,768 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000014 -0.13499977 0.34433948] -2025-05-21 15:52:14,772 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159079e+00 4.79368063e-07 -1.57079539e+00] -2025-05-21 15:52:14,778 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999977 -0.13495783 0.34407153] -2025-05-21 15:52:14,782 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999944e+02 -3.63110267e-03 -8.99999223e+01] -2025-05-21 15:52:14,783 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000009 -0.13499976 0.34433942] -2025-05-21 15:52:14,784 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 -2.22044605e-16 -1.57079539e+00] -2025-05-21 15:52:14,795 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000015 -0.13495467 0.34406337] -2025-05-21 15:52:14,796 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999799e+02 -3.87454148e-03 -8.99998355e+01] -2025-05-21 15:52:14,798 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999989 -0.13499988 0.34433923] -2025-05-21 15:52:14,820 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212 0. -1.57079579] -2025-05-21 15:52:14,826 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000001 -0.13495228 0.34405685] -2025-05-21 15:52:14,827 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999830e+02 -4.09244907e-03 -8.99998393e+01] -2025-05-21 15:52:14,828 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000031 -0.13499976 0.34433957] -2025-05-21 15:52:14,829 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 -4.58966198e-13 -1.57079486e+00] -2025-05-21 15:52:14,842 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000035 -0.13495003 0.3440521 ] -2025-05-21 15:52:14,851 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999797e+02 -4.28438253e-03 -8.99998165e+01] -2025-05-21 15:52:14,854 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000012 -0.13500012 0.34433969] -2025-05-21 15:52:14,855 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 -4.79369682e-07 -1.57079579e+00] -2025-05-21 15:52:14,859 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000016 -0.13494858 0.34404555] -2025-05-21 15:52:14,861 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999788e+02 -4.47874624e-03 -8.99998794e+01] -2025-05-21 15:52:14,863 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000012 -0.13499977 0.34433917] -2025-05-21 15:52:14,864 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 4.79368984e-07 -1.57079579e+00] -2025-05-21 15:52:14,877 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999982 -0.13494694 0.34404033] -2025-05-21 15:52:14,880 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999857e+02 -4.59517459e-03 -8.99999199e+01] -2025-05-21 15:52:14,882 - 测试日志 - INFO - log.py:106 - position current:[ 0.2500002 -0.13500035 0.3443392 ] -2025-05-21 15:52:14,884 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -4.79369237e-07 -1.57079779e+00] -2025-05-21 15:52:14,896 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999977 -0.13494535 0.34403608] -2025-05-21 15:52:14,900 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999982e+02 -4.68552200e-03 -8.99998748e+01] -2025-05-21 15:52:14,902 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000024 -0.13499988 0.34433972] -2025-05-21 15:52:14,905 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 4.59632332e-13 -1.57079633e+00] -2025-05-21 15:52:14,915 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000007 -0.13494476 0.34403548] -2025-05-21 15:52:14,917 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999748e+02 -4.81460245e-03 -8.99999671e+01] -2025-05-21 15:52:14,920 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999997 -0.13499963 0.34433902] -2025-05-21 15:52:14,935 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265 0. -1.57079486] -2025-05-21 15:52:14,942 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000002 -0.1349434 0.34403345] -2025-05-21 15:52:14,948 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999864e+02 -4.90160527e-03 -8.99998611e+01] -2025-05-21 15:52:14,951 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000018 -0.135 0.34433957] -2025-05-21 15:52:14,953 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 4.59632332e-13 -1.57079686e+00] -2025-05-21 15:52:14,959 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999988 -0.13494314 0.34403296] -2025-05-21 15:52:14,965 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999876e+02 -4.94822462e-03 -9.00000059e+01] -2025-05-21 15:52:14,968 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000002 -0.13499977 0.34433914] -2025-05-21 15:52:14,972 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 4.79368983e-07 -1.57079579e+00] -2025-05-21 15:52:14,977 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000007 -0.13494216 0.34403297] -2025-05-21 15:52:14,982 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999892e+02 -5.01241089e-03 -8.99999294e+01] -2025-05-21 15:52:14,983 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999969 -0.135 0.34433826] -2025-05-21 15:52:14,986 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159026 0. -1.57079633] -2025-05-21 15:52:14,997 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999989 -0.13494188 0.34403309] -2025-05-21 15:52:14,999 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999874e+02 -5.06492697e-03 -8.99999660e+01] -2025-05-21 15:52:15,001 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000006 -0.13500012 0.34433902] -2025-05-21 15:52:15,003 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -4.79368984e-07 -1.57079633e+00] -2025-05-21 15:52:15,012 - 测试日志 - INFO - log.py:106 - MassageRobot启动 -2025-05-21 15:52:15,018 - 测试日志 - INFO - log.py:106 - 机械臂控制线程启动 -2025-05-21 15:53:09,492 - 测试日志 - ERROR - log.py:106 - 传感器数据读取失败 -2025-05-21 15:53:09,495 - 测试日志 - ERROR - log.py:106 - 传感器线程数据读取失败-1 -2025-05-21 15:58:44,141 - 测试日志 - INFO - log.py:106 - 切换到finger_head按摩头 -2025-05-21 15:58:47,653 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999988 -0.13500023 0.34428386] -2025-05-21 15:58:47,655 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999848e+02 -5.77903395e-04 -9.00005484e+01] -2025-05-21 15:58:47,661 - 测试日志 - INFO - log.py:106 - 机械臂测量线程启动 -2025-05-21 15:58:47,664 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999994 -0.13500012 0.34433914] -2025-05-21 15:58:47,665 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -4.79368983e-07 -1.57079633e+00] -2025-05-21 15:58:47,668 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000007 -0.13499989 0.34423911] -2025-05-21 15:58:47,669 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999785e+02 -4.49178336e-04 -9.00002189e+01] -2025-05-21 15:58:47,671 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999969 -0.13500024 0.34433893] -2025-05-21 15:58:47,673 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212 0. -1.57079726] -2025-05-21 15:58:47,686 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999999 -0.13500024 0.3442021 ] -2025-05-21 15:58:47,688 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999780e+02 -5.79921912e-05 -9.00002018e+01] -2025-05-21 15:58:47,689 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999985 -0.135 0.3443392 ] -2025-05-21 15:58:47,691 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 -4.79368983e-07 -1.57079539e+00] -2025-05-21 15:58:47,704 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999614 -0.13500012 0.344171 ] -2025-05-21 15:58:47,706 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999771e+02 -2.04567679e-04 -9.00002492e+01] -2025-05-21 15:58:47,708 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000009 -0.13500035 0.34433911] -2025-05-21 15:58:47,710 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -9.58737777e-07 -1.57079633e+00] -2025-05-21 15:58:47,726 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999362 -0.13500036 0.34414719] -2025-05-21 15:58:47,730 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999998e+02 -4.74650528e-04 -9.00001259e+01] -2025-05-21 15:58:47,733 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000005 -0.13500035 0.34433923] -2025-05-21 15:58:47,738 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 -9.58737967e-07 -1.57079633e+00] -2025-05-21 15:58:47,744 - 测试日志 - INFO - log.py:106 - position command: [ 0.24998898 -0.13500035 0.3441271 ] -2025-05-21 15:58:47,746 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999729e+02 -1.44367945e-04 -9.00000674e+01] -2025-05-21 15:58:47,750 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999976 -0.13500035 0.34433875] -2025-05-21 15:58:47,753 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159132e+00 -1.43810625e-06 -1.57079579e+00] -2025-05-21 15:58:47,760 - 测试日志 - INFO - log.py:106 - position command: [ 0.24998664 -0.13500023 0.34411009] -2025-05-21 15:58:47,766 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999613e+02 -4.66798511e-04 -8.99999713e+01] -2025-05-21 15:58:47,769 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999976 -0.13500035 0.34433875] -2025-05-21 15:58:47,774 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159132e+00 -1.43810625e-06 -1.57079579e+00] -2025-05-21 15:58:47,787 - 测试日志 - INFO - log.py:106 - position command: [ 0.24998276 -0.13500059 0.34409684] -2025-05-21 15:58:47,790 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999690e+02 -3.16617949e-04 -9.00005529e+01] -2025-05-21 15:58:47,795 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.13500035 0.34433936] -2025-05-21 15:58:47,796 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -9.58738475e-07 -1.57079579e+00] -2025-05-21 15:58:47,800 - 测试日志 - INFO - log.py:106 - position command: [ 0.24997882 -0.13500034 0.3440861 ] -2025-05-21 15:58:47,802 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999673e+02 -6.40691785e-05 -9.00003452e+01] -2025-05-21 15:58:47,805 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000002 -0.135 0.34433914] -2025-05-21 15:58:47,813 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 -4.79368984e-07 -1.57079446e+00] -2025-05-21 15:58:47,817 - 测试日志 - INFO - log.py:106 - position command: [ 0.24997424 -0.13499988 0.34407692] -2025-05-21 15:58:47,822 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999479e+02 -4.74315845e-04 -9.00003813e+01] -2025-05-21 15:58:47,826 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000018 -0.13500035 0.34433975] -2025-05-21 15:58:47,829 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159079e+00 -9.58740761e-07 -1.57079539e+00] -2025-05-21 15:58:47,845 - 测试日志 - INFO - log.py:106 - position command: [ 0.24996966 -0.13500001 0.34406945] -2025-05-21 15:58:47,848 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999551e+02 -1.58267278e-04 -9.00000494e+01] -2025-05-21 15:58:47,851 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000011 -0.13500011 0.34433905] -2025-05-21 15:58:47,855 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -9.58737522e-07 -1.57079539e+00] -2025-05-21 15:58:47,864 - 测试日志 - INFO - log.py:106 - position command: [ 0.24996798 -0.13500012 0.34406327] -2025-05-21 15:58:47,868 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999607e+02 -2.48239518e-04 -9.00004534e+01] -2025-05-21 15:58:47,873 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000002 -0.13500011 0.34433914] -2025-05-21 15:58:47,881 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 -9.58737967e-07 -1.57079446e+00] -2025-05-21 15:58:47,885 - 测试日志 - INFO - log.py:106 - position command: [ 0.24996562 -0.13500012 0.3440584 ] -2025-05-21 15:58:47,889 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999544e+02 -3.61674067e-04 -9.00004180e+01] -2025-05-21 15:58:47,893 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000018 -0.13500023 0.34433942] -2025-05-21 15:58:47,902 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 -9.58738475e-07 -1.57079579e+00] -2025-05-21 15:58:47,906 - 测试日志 - INFO - log.py:106 - position command: [ 0.2499656 -0.13500013 0.34405355] -2025-05-21 15:58:47,914 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999896e+02 -4.46063469e-04 -9.00005242e+01] -2025-05-21 15:58:47,919 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999988 -0.13500023 0.34433911] -2025-05-21 15:58:47,921 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -9.58736634e-07 -1.57079486e+00] -2025-05-21 15:58:47,926 - 测试日志 - INFO - log.py:106 - position command: [ 0.2499676 -0.13500012 0.34405092] -2025-05-21 15:58:47,929 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999608e+02 -1.36777719e-04 -9.00004099e+01] -2025-05-21 15:58:47,934 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999998 -0.1350006 0.34433923] -2025-05-21 15:58:47,937 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 -1.43810701e-06 -1.57079633e+00] -2025-05-21 15:58:47,946 - 测试日志 - INFO - log.py:106 - position command: [ 0.24996767 -0.13500024 0.34404853] -2025-05-21 15:58:47,949 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999531e+02 -5.83350517e-04 -9.00000663e+01] -2025-05-21 15:58:47,955 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000012 -0.13500023 0.34433936] -2025-05-21 15:58:47,958 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.43810739e-06 -1.57079486e+00] -2025-05-21 15:58:47,969 - 测试日志 - INFO - log.py:106 - position command: [ 0.24996561 -0.13500024 0.34404297] -2025-05-21 15:58:47,973 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999949e+02 -5.32397919e-05 -9.00001815e+01] -2025-05-21 15:58:47,983 - 测试日志 - INFO - log.py:106 - position current:[ 0.2499996 -0.13500047 0.34433899] -2025-05-21 15:58:47,989 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.43810651e-06 -1.57079579e+00] -2025-05-21 15:58:48,000 - 测试日志 - INFO - log.py:106 - position command: [ 0.24996361 -0.13500059 0.34404026] -2025-05-21 15:58:48,003 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999789e+02 -9.07766589e-05 -9.00004640e+01] -2025-05-21 15:58:48,007 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999988 -0.13500035 0.34433929] -2025-05-21 15:58:48,014 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 -9.58737967e-07 -1.57079579e+00] -2025-05-21 15:58:48,018 - 测试日志 - INFO - log.py:106 - position command: [ 0.24996585 -0.13500011 0.34403805] -2025-05-21 15:58:48,022 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999456e+02 -1.58691728e-04 -9.00004429e+01] -2025-05-21 15:58:48,030 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999986 -0.13500035 0.34433884] -2025-05-21 15:58:48,034 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159132e+00 -9.58737332e-07 -1.57079633e+00] -2025-05-21 15:58:48,038 - 测试日志 - INFO - log.py:106 - position command: [ 0.24996868 -0.13500035 0.34403638] -2025-05-21 15:58:48,039 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999723e+02 -1.75630664e-04 -9.00001215e+01] -2025-05-21 15:58:48,040 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.135 0.34433908] -2025-05-21 15:58:48,043 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 4.59854377e-13 -1.57079539e+00] -2025-05-21 15:58:48,094 - 测试日志 - INFO - log.py:106 - 机械臂控制线程启动 -2025-05-21 15:58:48,108 - 测试日志 - INFO - log.py:106 - MassageRobot启动 -2025-05-21 16:05:44,925 - 测试日志 - INFO - log.py:106 - 切换到finger_head按摩头 -2025-05-21 16:05:48,433 - 测试日志 - INFO - log.py:106 - 机械臂测量线程启动 -2025-05-21 16:05:48,436 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000003 -0.1350006 0.34428401] -2025-05-21 16:05:48,437 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999969e+02 -8.23974988e-05 -9.00001373e+01] -2025-05-21 16:05:48,438 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000017 -0.13500035 0.34433911] -2025-05-21 16:05:48,440 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -9.58737332e-07 -1.57079819e+00] -2025-05-21 16:05:48,442 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000016 -0.13500036 0.34423894] -2025-05-21 16:05:48,443 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999970e+02 -5.49721085e-05 -9.00000495e+01] -2025-05-21 16:05:48,446 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000002 -0.13500047 0.3443385 ] -2025-05-21 16:05:48,448 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159079e+00 -9.58741650e-07 -1.57079926e+00] -2025-05-21 16:05:48,460 - 测试日志 - INFO - log.py:106 - position command: [ 0.2500001 -0.13500037 0.34420202] -2025-05-21 16:05:48,462 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.80000000e+02 -5.06637160e-05 -8.99999768e+01] -2025-05-21 16:05:48,467 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000021 -0.13500023 0.34433884] -2025-05-21 16:05:48,469 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -9.58739364e-07 -1.57079779e+00] -2025-05-21 16:05:48,479 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000023 -0.13500047 0.34417679] -2025-05-21 16:05:48,481 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999894e+02 -6.39678073e-05 -8.99999284e+01] -2025-05-21 16:05:48,486 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000005 -0.13500023 0.34433905] -2025-05-21 16:05:48,489 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -9.58740316e-07 -1.57079872e+00] -2025-05-21 16:05:48,499 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000008 -0.13500061 0.34415271] -2025-05-21 16:05:48,503 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999970e+02 -2.28591308e-06 -8.99999867e+01] -2025-05-21 16:05:48,508 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.13500023 0.34433893] -2025-05-21 16:05:48,510 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -9.58738666e-07 -1.57079779e+00] -2025-05-21 16:05:48,521 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000005 -0.13500037 0.34413613] -2025-05-21 16:05:48,524 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999952e+02 1.36337308e-05 -8.99999958e+01] -2025-05-21 16:05:48,529 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999992 -0.13500035 0.34433884] -2025-05-21 16:05:48,532 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -9.58738666e-07 -1.57079819e+00] -2025-05-21 16:05:48,541 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999992 -0.13500038 0.34412076] -2025-05-21 16:05:48,544 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999996e+02 4.55943375e-05 -9.00000068e+01] -2025-05-21 16:05:48,549 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999998 -0.13500047 0.3443389 ] -2025-05-21 16:05:48,552 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -1.43810841e-06 -1.57079819e+00] -2025-05-21 16:05:48,558 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000288 -0.13500062 0.34410788] -2025-05-21 16:05:48,562 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999734e+02 1.58673766e-05 -8.99999955e+01] -2025-05-21 16:05:48,565 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000035 -0.13500047 0.34433868] -2025-05-21 16:05:48,570 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159079e+00 -9.58740761e-07 -1.57079872e+00] -2025-05-21 16:05:48,582 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000801 -0.1350005 0.34409622] -2025-05-21 16:05:48,586 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999249e+02 3.70534526e-05 -9.00000601e+01] -2025-05-21 16:05:48,591 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000005 -0.13500023 0.34433908] -2025-05-21 16:05:48,596 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -4.79368983e-07 -1.57079779e+00] -2025-05-21 16:05:48,599 - 测试日志 - INFO - log.py:106 - position command: [ 0.2500101 -0.13500026 0.34408986] -2025-05-21 16:05:48,603 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999144e+02 5.61403486e-05 -9.00000173e+01] -2025-05-21 16:05:48,605 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000005 -0.13500035 0.3443389 ] -2025-05-21 16:05:48,607 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -1.43810790e-06 -1.57079726e+00] -2025-05-21 16:05:48,618 - 测试日志 - INFO - log.py:106 - position command: [ 0.25001132 -0.13500038 0.34408348] -2025-05-21 16:05:48,621 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999089e+02 1.96093174e-05 -9.00000270e+01] -2025-05-21 16:05:48,625 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000009 -0.13500035 0.34433945] -2025-05-21 16:05:48,627 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -4.79368317e-07 -1.57079872e+00] -2025-05-21 16:05:48,641 - 测试日志 - INFO - log.py:106 - position command: [ 0.25001109 -0.13500039 0.34407722] -2025-05-21 16:05:48,646 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999085e+02 3.79003875e-05 -9.00000889e+01] -2025-05-21 16:05:48,651 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.13500047 0.34433917] -2025-05-21 16:05:48,656 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.43810625e-06 -1.57079819e+00] -2025-05-21 16:05:48,665 - 测试日志 - INFO - log.py:106 - position command: [ 0.25001068 -0.13500051 0.3440715 ] -2025-05-21 16:05:48,672 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999159e+02 1.78778418e-06 -9.00000963e+01] -2025-05-21 16:05:48,676 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999992 -0.1350006 0.34433936] -2025-05-21 16:05:48,687 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.43810676e-06 -1.57079779e+00] -2025-05-21 16:05:48,693 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000927 -0.13500062 0.34406883] -2025-05-21 16:05:48,697 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999224e+02 -3.41951233e-05 -9.00000498e+01] -2025-05-21 16:05:48,703 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999998 -0.13500023 0.3443389 ] -2025-05-21 16:05:48,709 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -4.79370380e-07 -1.57079819e+00] -2025-05-21 16:05:48,715 - 测试日志 - INFO - log.py:106 - position command: [ 0.2500082 -0.13500026 0.34406528] -2025-05-21 16:05:48,721 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999230e+02 1.31252193e-05 -9.00000784e+01] -2025-05-21 16:05:48,725 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000029 -0.13500012 0.34433978] -2025-05-21 16:05:48,732 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159026e+00 3.44679840e-12 -1.57079819e+00] -2025-05-21 16:05:48,738 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000741 -0.13500016 0.34406274] -2025-05-21 16:05:48,746 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999444e+02 3.39259212e-05 -9.00000605e+01] -2025-05-21 16:05:48,749 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999974 -0.13500023 0.34433829] -2025-05-21 16:05:48,750 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159079e+00 -4.79369904e-07 -1.57079726e+00] -2025-05-21 16:05:48,753 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000641 -0.13500027 0.34405918] -2025-05-21 16:05:48,759 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999530e+02 8.35033640e-07 -9.00000646e+01] -2025-05-21 16:05:48,763 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.13500012 0.34433902] -2025-05-21 16:05:48,766 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -4.79368984e-07 -1.57079633e+00] -2025-05-21 16:05:48,772 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000547 -0.13500017 0.34405917] -2025-05-21 16:05:48,776 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999622e+02 4.04822120e-05 -9.00000681e+01] -2025-05-21 16:05:48,779 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999986 -0.13500035 0.34433917] -2025-05-21 16:05:48,781 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -4.79369460e-07 -1.57079819e+00] -2025-05-21 16:05:48,793 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000451 -0.1350004 0.34406121] -2025-05-21 16:05:48,795 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999690e+02 -2.61108421e-05 -9.00000405e+01] -2025-05-21 16:05:48,801 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000021 -0.13500012 0.34433884] -2025-05-21 16:05:48,809 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -4.79370380e-07 -1.57079779e+00] -2025-05-21 16:05:48,813 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000396 -0.13500018 0.34406024] -2025-05-21 16:05:48,817 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999613e+02 -7.12208265e-06 -9.00000564e+01] -2025-05-21 16:05:48,821 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000027 -0.135 0.34433932] -2025-05-21 16:05:48,832 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265 0. -1.57079726] -2025-05-21 16:05:48,833 - 测试日志 - INFO - log.py:106 - MassageRobot启动 -2025-05-21 16:05:48,835 - 测试日志 - INFO - log.py:106 - 机械臂控制线程启动 -2025-05-21 16:06:14,414 - 测试日志 - ERROR - log.py:106 - 传感器数据读取失败 -2025-05-21 16:06:14,416 - 测试日志 - ERROR - log.py:106 - 传感器线程数据读取失败-1 -2025-05-21 16:07:20,684 - 测试日志 - INFO - log.py:106 - 切换到finger_head按摩头 -2025-05-21 16:07:24,200 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999974 -0.13500035 0.34428417] -2025-05-21 16:07:24,201 - 测试日志 - INFO - log.py:106 - 机械臂测量线程启动 -2025-05-21 16:07:24,205 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999939e+02 -1.09863307e-04 -8.99999466e+01] -2025-05-21 16:07:24,208 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000012 -0.13500035 0.34433936] -2025-05-21 16:07:24,234 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 -1.43810701e-06 -1.57079633e+00] -2025-05-21 16:07:24,245 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999982 -0.13500069 0.34423864] -2025-05-21 16:07:24,246 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999939e+02 -1.37226555e-04 -9.00000343e+01] -2025-05-21 16:07:24,248 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000017 -0.13500035 0.34433939] -2025-05-21 16:07:24,249 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 -1.91747593e-06 -1.57079539e+00] -2025-05-21 16:07:24,256 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000011 -0.13500045 0.34420263] -2025-05-21 16:07:24,259 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999924e+02 -1.09716625e-04 -9.00000172e+01] -2025-05-21 16:07:24,261 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000011 -0.13500035 0.34433908] -2025-05-21 16:07:24,264 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -1.43810701e-06 -1.57079633e+00] -2025-05-21 16:07:24,274 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999989 -0.13500034 0.34417244] -2025-05-21 16:07:24,276 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999939e+02 -9.54344781e-05 -9.00000226e+01] -2025-05-21 16:07:24,284 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999966 -0.13500047 0.34433932] -2025-05-21 16:07:24,286 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.43810701e-06 -1.57079686e+00] -2025-05-21 16:07:24,296 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999966 -0.13500047 0.34414739] -2025-05-21 16:07:24,300 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999969e+02 -5.99004250e-05 -9.00000849e+01] -2025-05-21 16:07:24,304 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999969 -0.13499988 0.34433887] -2025-05-21 16:07:24,307 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -9.58738221e-07 -1.57079539e+00] -2025-05-21 16:07:24,313 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000039 -0.13500035 0.34412754] -2025-05-21 16:07:24,316 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999893e+02 -5.55823507e-05 -9.00000230e+01] -2025-05-21 16:07:24,324 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999988 -0.13500047 0.34433929] -2025-05-21 16:07:24,327 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -1.43810701e-06 -1.57079686e+00] -2025-05-21 16:07:24,332 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000037 -0.13500035 0.34411101] -2025-05-21 16:07:24,336 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999893e+02 -5.36656777e-05 -9.00000208e+01] -2025-05-21 16:07:24,339 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999992 -0.13500046 0.34433868] -2025-05-21 16:07:24,343 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159132e+00 -1.91747530e-06 -1.57079579e+00] -2025-05-21 16:07:24,353 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000008 -0.13500045 0.34409715] -2025-05-21 16:07:24,354 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999970e+02 -1.08108332e-04 -8.99999949e+01] -2025-05-21 16:07:24,356 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000012 -0.13500046 0.34433936] -2025-05-21 16:07:24,361 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -2.39684523e-06 -1.57079539e+00] -2025-05-21 16:07:24,371 - 测试日志 - INFO - log.py:106 - position command: [ 0.2500001 -0.13500047 0.34408588] -2025-05-21 16:07:24,373 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999939e+02 -5.38240956e-05 -9.00000753e+01] -2025-05-21 16:07:24,376 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000008 -0.13500011 0.34433948] -2025-05-21 16:07:24,381 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 -9.58738475e-07 -1.57079539e+00] -2025-05-21 16:07:24,391 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999995 -0.13500045 0.34407672] -2025-05-21 16:07:24,398 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999970e+02 -8.25003820e-05 -9.00000107e+01] -2025-05-21 16:07:24,405 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999966 -0.13500046 0.34433914] -2025-05-21 16:07:24,406 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.91747581e-06 -1.57079579e+00] -2025-05-21 16:07:24,416 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000012 -0.13500035 0.34406959] -2025-05-21 16:07:24,419 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999969e+02 -2.96485374e-05 -9.00000448e+01] -2025-05-21 16:07:24,422 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.13500023 0.34433875] -2025-05-21 16:07:24,423 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159132e+00 -1.43810701e-06 -1.57079579e+00] -2025-05-21 16:07:24,429 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999988 -0.13500047 0.34406341] -2025-05-21 16:07:24,432 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999970e+02 -5.89037531e-05 -9.00000481e+01] -2025-05-21 16:07:24,435 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000011 -0.13500011 0.34433987] -2025-05-21 16:07:24,439 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14158972e+00 -1.43810701e-06 -1.57079539e+00] -2025-05-21 16:07:24,448 - 测试日志 - INFO - log.py:106 - position command: [ 0.25 -0.13500035 0.34405819] -2025-05-21 16:07:24,449 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999999e+02 -3.33632770e-05 -9.00000589e+01] -2025-05-21 16:07:24,451 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.13500035 0.34433902] -2025-05-21 16:07:24,460 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.43810676e-06 -1.57079579e+00] -2025-05-21 16:07:24,474 - 测试日志 - INFO - log.py:106 - position command: [ 0.25 -0.13500035 0.34405414] -2025-05-21 16:07:24,480 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999999e+02 -3.52775943e-05 -9.00000745e+01] -2025-05-21 16:07:24,483 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999974 -0.13500023 0.34433942] -2025-05-21 16:07:24,485 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 -1.43810651e-06 -1.57079633e+00] -2025-05-21 16:07:24,490 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999979 -0.13500035 0.34405101] -2025-05-21 16:07:24,497 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999970e+02 -6.45265723e-05 -9.00000519e+01] -2025-05-21 16:07:24,498 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000006 -0.13500035 0.34433951] -2025-05-21 16:07:24,500 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.43810727e-06 -1.57079579e+00] -2025-05-21 16:07:24,506 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000016 -0.13500012 0.34404788] -2025-05-21 16:07:24,511 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999894e+02 -3.87130463e-05 -9.00000590e+01] -2025-05-21 16:07:24,513 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999998 -0.1350006 0.34433939] -2025-05-21 16:07:24,515 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -1.43810701e-06 -1.57079779e+00] -2025-05-21 16:07:24,527 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999994 -0.1350006 0.34404638] -2025-05-21 16:07:24,536 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999999e+02 -5.00633925e-05 -9.00001897e+01] -2025-05-21 16:07:24,541 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999989 -0.13500023 0.34433926] -2025-05-21 16:07:24,545 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -9.58738221e-07 -1.57079579e+00] -2025-05-21 16:07:24,550 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999989 -0.13500035 0.34404432] -2025-05-21 16:07:24,563 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999999e+02 -4.05435983e-05 -9.00001133e+01] -2025-05-21 16:07:24,567 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999985 -0.13500047 0.34433841] -2025-05-21 16:07:24,571 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159079e+00 -9.58736126e-07 -1.57079633e+00] -2025-05-21 16:07:24,574 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999985 -0.13500035 0.34404277] -2025-05-21 16:07:24,579 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999971e+02 -5.27608864e-05 -9.00000311e+01] -2025-05-21 16:07:24,581 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999968 -0.13500047 0.3443396 ] -2025-05-21 16:07:24,583 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159079e+00 -1.43810790e-06 -1.57079633e+00] -2025-05-21 16:07:24,594 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999974 -0.13500024 0.34404155] -2025-05-21 16:07:24,599 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999971e+02 3.76935964e-05 -9.00001761e+01] -2025-05-21 16:07:24,601 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999989 -0.13500035 0.34433893] -2025-05-21 16:07:24,603 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.91747593e-06 -1.57079579e+00] -2025-05-21 16:07:24,612 - 测试日志 - INFO - log.py:106 - 机械臂控制线程启动 -2025-05-21 16:07:24,615 - 测试日志 - INFO - log.py:106 - MassageRobot启动 -2025-05-21 16:09:10,075 - 测试日志 - INFO - log.py:106 - 切换到finger_head按摩头 -2025-05-21 16:09:13,583 - 测试日志 - INFO - log.py:106 - 机械臂测量线程启动 -2025-05-21 16:09:13,584 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000006 -0.13499977 0.34428408] -2025-05-21 16:09:13,586 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999969e+02 5.49317047e-05 -9.00001373e+01] -2025-05-21 16:09:13,590 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000006 -0.13499977 0.34433917] -2025-05-21 16:09:13,592 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 9.58739110e-07 -1.57079872e+00] -2025-05-21 16:09:13,594 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999966 -0.13500021 0.34423885] -2025-05-21 16:09:13,596 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999942e+02 -2.50178599e-05 -9.00001373e+01] -2025-05-21 16:09:13,597 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999928 -0.13500047 0.34433893] -2025-05-21 16:09:13,597 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -2.29816166e-12 -1.57080059e+00] -2025-05-21 16:09:13,613 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999991 -0.13499986 0.34420185] -2025-05-21 16:09:13,615 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999930e+02 -2.28135520e-05 -9.00000562e+01] -2025-05-21 16:09:13,615 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999988 -0.135 0.34433929] -2025-05-21 16:09:13,616 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 4.79368983e-07 -1.57079872e+00] -2025-05-21 16:09:13,632 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999687 -0.1350001 0.34417147] -2025-05-21 16:09:13,634 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79998566e+02 5.61845121e-06 -9.00001691e+01] -2025-05-21 16:09:13,635 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000026 -0.13499976 0.34433969] -2025-05-21 16:09:13,636 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 2.75690581e-12 -1.57079726e+00] -2025-05-21 16:09:13,651 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999584 -0.13500032 0.34414739] -2025-05-21 16:09:13,652 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79997701e+02 -2.01126944e-05 -9.00000259e+01] -2025-05-21 16:09:13,653 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000029 -0.13499965 0.34433911] -2025-05-21 16:09:13,653 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 9.58738919e-07 -1.57079779e+00] -2025-05-21 16:09:13,670 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999519 -0.13499974 0.34412736] -2025-05-21 16:09:13,671 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79997336e+02 1.42020786e-04 -8.99999139e+01] -2025-05-21 16:09:13,671 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999988 -0.13499988 0.34433881] -2025-05-21 16:09:13,672 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 4.79367142e-07 -1.57079819e+00] -2025-05-21 16:09:13,689 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999453 -0.13500009 0.34411082] -2025-05-21 16:09:13,690 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79997062e+02 2.03173219e-04 -8.99998882e+01] -2025-05-21 16:09:13,691 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000008 -0.13499988 0.34433929] -2025-05-21 16:09:13,692 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 2.29793962e-12 -1.57079819e+00] -2025-05-21 16:09:13,708 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999432 -0.13499996 0.34409643] -2025-05-21 16:09:13,711 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79997179e+02 1.40662469e-04 -8.99998708e+01] -2025-05-21 16:09:13,713 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999977 -0.13500012 0.34433902] -2025-05-21 16:09:13,715 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -4.79368983e-07 -1.57079726e+00] -2025-05-21 16:09:13,726 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999459 -0.13500008 0.34408579] -2025-05-21 16:09:13,730 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79997236e+02 7.04614615e-05 -8.99998208e+01] -2025-05-21 16:09:13,733 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000006 -0.13500012 0.34433917] -2025-05-21 16:09:13,735 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -4.79367841e-07 -1.57079872e+00] -2025-05-21 16:09:13,744 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999509 -0.13500008 0.34407674] -2025-05-21 16:09:13,746 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79997529e+02 -1.06496486e-04 -8.99998200e+01] -2025-05-21 16:09:13,748 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999989 -0.13500012 0.34433926] -2025-05-21 16:09:13,749 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -4.79368063e-07 -1.57079819e+00] -2025-05-21 16:09:13,764 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999571 -0.13500008 0.34406954] -2025-05-21 16:09:13,765 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79997855e+02 -2.22739838e-04 -8.99998610e+01] -2025-05-21 16:09:13,766 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.13500012 0.34433926] -2025-05-21 16:09:13,767 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 9.19042620e-13 -1.57079872e+00] -2025-05-21 16:09:13,782 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999647 -0.13500007 0.3440636 ] -2025-05-21 16:09:13,783 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79998079e+02 -3.83143260e-04 -8.99998639e+01] -2025-05-21 16:09:13,784 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999977 -0.135 0.34433887] -2025-05-21 16:09:13,785 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -4.79370380e-07 -1.57079726e+00] -2025-05-21 16:09:13,800 - 测试日志 - INFO - log.py:106 - position command: [ 0.2499968 -0.13499984 0.34405819] -2025-05-21 16:09:13,801 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79998445e+02 -4.59585472e-04 -8.99997991e+01] -2025-05-21 16:09:13,802 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999969 -0.135 0.34433841] -2025-05-21 16:09:13,803 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159026e+00 -4.59610128e-12 -1.57079819e+00] -2025-05-21 16:09:13,821 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999738 -0.13499972 0.34405405] -2025-05-21 16:09:13,822 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79998642e+02 -5.22129454e-04 -8.99997853e+01] -2025-05-21 16:09:13,824 - 测试日志 - INFO - log.py:106 - position current:[ 0.2499996 -0.135 0.34433914] -2025-05-21 16:09:13,825 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 4.79368063e-07 -1.57079872e+00] -2025-05-21 16:09:13,838 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999762 -0.13499985 0.34405095] -2025-05-21 16:09:13,841 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79998795e+02 -6.06519852e-04 -8.99998167e+01] -2025-05-21 16:09:13,845 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999954 -0.135 0.34433893] -2025-05-21 16:09:13,847 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -1.83808524e-12 -1.57079819e+00] -2025-05-21 16:09:13,856 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999807 -0.13499973 0.34404814] -2025-05-21 16:09:13,858 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999011e+02 -6.26628902e-04 -8.99998773e+01] -2025-05-21 16:09:13,860 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000017 -0.13500035 0.3443396 ] -2025-05-21 16:09:13,864 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -9.58737078e-07 -1.57079872e+00] -2025-05-21 16:09:13,875 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999846 -0.13500009 0.34404622] -2025-05-21 16:09:13,879 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999142e+02 -6.07370865e-04 -8.99999870e+01] -2025-05-21 16:09:13,880 - 测试日志 - INFO - log.py:106 - position current:[ 0.2500006 -0.13499976 0.34433972] -2025-05-21 16:09:13,881 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 3.44657636e-12 -1.57079779e+00] -2025-05-21 16:09:13,897 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999925 -0.13499973 0.34404487] -2025-05-21 16:09:13,898 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999207e+02 -6.67890883e-04 -8.99999299e+01] -2025-05-21 16:09:13,899 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999982 -0.13500012 0.34433881] -2025-05-21 16:09:13,900 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -2.75734990e-12 -1.57079966e+00] -2025-05-21 16:09:13,917 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999842 -0.13500031 0.34404266] -2025-05-21 16:09:13,919 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999429e+02 -5.94917744e-04 -9.00000572e+01] -2025-05-21 16:09:13,920 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000008 -0.13499976 0.34433929] -2025-05-21 16:09:13,922 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265 0. -1.57079779] -2025-05-21 16:09:13,933 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999915 -0.13499973 0.34404174] -2025-05-21 16:09:13,935 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999498e+02 -4.79777405e-04 -8.99999723e+01] -2025-05-21 16:09:13,936 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999971 -0.13499988 0.34433902] -2025-05-21 16:09:13,938 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 4.79366698e-07 -1.57079872e+00] -2025-05-21 16:09:13,950 - 测试日志 - INFO - log.py:106 - MassageRobot启动 -2025-05-21 16:09:13,951 - 测试日志 - INFO - log.py:106 - 机械臂控制线程启动 -2025-05-21 16:12:13,004 - 测试日志 - INFO - log.py:106 - 切换到finger_head按摩头 -2025-05-21 16:12:16,520 - 测试日志 - INFO - log.py:106 - 机械臂测量线程启动 -2025-05-21 16:12:16,523 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000005 -0.135 0.34428395] -2025-05-21 16:12:16,524 - 测试日志 - INFO - log.py:106 - orientation command: [179.99993896 0. -90. ] -2025-05-21 16:12:16,526 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999976 -0.13500012 0.34433875] -2025-05-21 16:12:16,528 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159132e+00 -4.79368983e-07 -1.57079633e+00] -2025-05-21 16:12:16,530 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999985 -0.13500022 0.34423935] -2025-05-21 16:12:16,531 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999944e+02 -5.26604994e-05 -9.00003172e+01] -2025-05-21 16:12:16,532 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.13500012 0.34433908] -2025-05-21 16:12:16,532 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -4.79369237e-07 -1.57079686e+00] -2025-05-21 16:12:16,548 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999974 -0.13500022 0.34420174] -2025-05-21 16:12:16,548 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999965e+02 -7.72613460e-05 -9.00006654e+01] -2025-05-21 16:12:16,549 - 测试日志 - INFO - log.py:106 - position current:[ 0.2500002 -0.13500024 0.34433905] -2025-05-21 16:12:16,550 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -2.29816166e-13 -1.57079779e+00] -2025-05-21 16:12:16,567 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999967 -0.13499999 0.34417131] -2025-05-21 16:12:16,569 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999890e+02 4.75540197e-06 -9.00010047e+01] -2025-05-21 16:12:16,570 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000011 -0.13500023 0.34433923] -2025-05-21 16:12:16,573 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -9.58737967e-07 -1.57079686e+00] -2025-05-21 16:12:16,585 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000008 -0.13499999 0.34414711] -2025-05-21 16:12:16,587 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999966e+02 -2.22906095e-05 -9.00013206e+01] -2025-05-21 16:12:16,590 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999986 -0.13500023 0.34433884] -2025-05-21 16:12:16,592 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159132e+00 -4.79369682e-07 -1.57079726e+00] -2025-05-21 16:12:16,603 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999996 -0.1350001 0.34412684] -2025-05-21 16:12:16,608 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999937e+02 -4.85478599e-05 -9.00015890e+01] -2025-05-21 16:12:16,611 - 测试日志 - INFO - log.py:106 - position current:[ 0.2500005 -0.13500011 0.34433926] -2025-05-21 16:12:16,612 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 -9.58737967e-07 -1.57079579e+00] -2025-05-21 16:12:16,625 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999984 -0.13500034 0.34411032] -2025-05-21 16:12:16,626 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999937e+02 -4.74921635e-05 -9.00018301e+01] -2025-05-21 16:12:16,628 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999988 -0.135 0.34433896] -2025-05-21 16:12:16,628 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159 0. -1.57079633] -2025-05-21 16:12:16,643 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999975 -0.13500021 0.34409715] -2025-05-21 16:12:16,644 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999974e+02 -4.66047476e-05 -9.00019559e+01] -2025-05-21 16:12:16,645 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000029 -0.135 0.34433929] -2025-05-21 16:12:16,646 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -4.79368983e-07 -1.57079579e+00] -2025-05-21 16:12:16,660 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000002 -0.1350001 0.3440856 ] -2025-05-21 16:12:16,661 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999965e+02 -1.93754096e-05 -9.00021377e+01] -2025-05-21 16:12:16,662 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000011 -0.13500012 0.34433923] -2025-05-21 16:12:16,663 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -4.79368983e-07 -1.57079686e+00] -2025-05-21 16:12:16,680 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000012 -0.1350001 0.34407694] -2025-05-21 16:12:16,681 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999995e+02 -1.96039975e-05 -9.00023414e+01] -2025-05-21 16:12:16,682 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000018 -0.13500023 0.34433942] -2025-05-21 16:12:16,684 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 -9.58737967e-07 -1.57079633e+00] -2025-05-21 16:12:16,697 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000019 -0.13500021 0.34406971] -2025-05-21 16:12:16,698 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999947e+02 -4.63085979e-05 -9.00024721e+01] -2025-05-21 16:12:16,700 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999994 -0.13500023 0.34433948] -2025-05-21 16:12:16,700 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 -4.79368063e-07 -1.57079779e+00] -2025-05-21 16:12:16,716 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000002 -0.13499986 0.34406319] -2025-05-21 16:12:16,717 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999991e+02 7.42518536e-06 -9.00026557e+01] -2025-05-21 16:12:16,717 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000041 -0.135 0.34433917] -2025-05-21 16:12:16,718 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -9.58737967e-07 -1.57079539e+00] -2025-05-21 16:12:16,733 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000025 -0.13499999 0.3440585 ] -2025-05-21 16:12:16,734 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999991e+02 -4.67143273e-05 -9.00027044e+01] -2025-05-21 16:12:16,735 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999979 -0.135 0.34433865] -2025-05-21 16:12:16,736 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -9.58737967e-07 -1.57079539e+00] -2025-05-21 16:12:16,754 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000012 -0.1350001 0.34405456] -2025-05-21 16:12:16,758 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999980e+02 -4.60242387e-05 -9.00028496e+01] -2025-05-21 16:12:16,761 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.13500011 0.34433868] -2025-05-21 16:12:16,762 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159132e+00 -9.58740062e-07 -1.57079726e+00] -2025-05-21 16:12:16,773 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999984 -0.13500021 0.34405112] -2025-05-21 16:12:16,774 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999960e+02 -4.54393431e-05 -9.00030299e+01] -2025-05-21 16:12:16,775 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000041 -0.13500011 0.34433951] -2025-05-21 16:12:16,777 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 -9.58737078e-07 -1.57079686e+00] -2025-05-21 16:12:16,791 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000018 -0.13499999 0.34404834] -2025-05-21 16:12:16,792 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999981e+02 -4.49290793e-05 -9.00029797e+01] -2025-05-21 16:12:16,794 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000008 -0.13500035 0.34433899] -2025-05-21 16:12:16,795 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -4.79369904e-07 -1.57079819e+00] -2025-05-21 16:12:16,808 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000016 -0.13500009 0.34404618] -2025-05-21 16:12:16,811 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999989e+02 -4.44982221e-05 -9.00030760e+01] -2025-05-21 16:12:16,813 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000027 -0.13500023 0.34433917] -2025-05-21 16:12:16,814 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -9.58737967e-07 -1.57079726e+00] -2025-05-21 16:12:16,828 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000027 -0.13500021 0.3440443 ] -2025-05-21 16:12:16,829 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999989e+02 1.26433554e-04 -9.00030486e+01] -2025-05-21 16:12:16,831 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.13500011 0.3443385 ] -2025-05-21 16:12:16,832 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159132e+00 -9.58738665e-07 -1.57079633e+00] -2025-05-21 16:12:16,847 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000022 -0.13500033 0.34404341] -2025-05-21 16:12:16,849 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999938e+02 2.20213531e-04 -9.00028640e+01] -2025-05-21 16:12:16,850 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000035 -0.13499998 0.34433896] -2025-05-21 16:12:16,851 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.43810739e-06 -1.57079579e+00] -2025-05-21 16:12:16,865 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000033 -0.13500009 0.34404185] -2025-05-21 16:12:16,867 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999984e+02 2.36819827e-04 -9.00026877e+01] -2025-05-21 16:12:16,869 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000003 -0.13500047 0.34433878] -2025-05-21 16:12:16,878 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159132e+00 -1.43810905e-06 -1.57079819e+00] -2025-05-21 16:12:16,884 - 测试日志 - INFO - log.py:106 - 机械臂控制线程启动 -2025-05-21 16:12:16,884 - 测试日志 - INFO - log.py:106 - MassageRobot启动 -2025-05-21 16:14:20,742 - 测试日志 - INFO - log.py:106 - 切换到finger_head按摩头 -2025-05-21 16:14:24,256 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999977 -0.1350006 0.34428417] -2025-05-21 16:14:24,257 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999969e+02 -8.23974478e-05 -9.00000839e+01] -2025-05-21 16:14:24,265 - 测试日志 - INFO - log.py:106 - 机械臂测量线程启动 -2025-05-21 16:14:24,265 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000006 -0.13500058 0.34433951] -2025-05-21 16:14:24,268 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 -1.91747593e-06 -1.57079686e+00] -2025-05-21 16:14:24,269 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999999 -0.13500058 0.34424903] -2025-05-21 16:14:24,270 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999973e+02 -1.08891452e-04 -9.00000028e+01] -2025-05-21 16:14:24,271 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.13500058 0.34433917] -2025-05-21 16:14:24,272 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.91747619e-06 -1.57079633e+00] -2025-05-21 16:14:24,288 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000016 -0.13500036 0.34421537] -2025-05-21 16:14:24,289 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999953e+02 -8.15226491e-05 -9.00000567e+01] -2025-05-21 16:14:24,290 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000018 -0.13500035 0.34433923] -2025-05-21 16:14:24,292 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 -1.43810613e-06 -1.57079726e+00] -2025-05-21 16:14:24,309 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999981 -0.13500037 0.34418561] -2025-05-21 16:14:24,310 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999822e+02 -8.16056663e-05 -9.00000571e+01] -2025-05-21 16:14:24,311 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000023 -0.13500058 0.34433963] -2025-05-21 16:14:24,312 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 -1.91747466e-06 -1.57079779e+00] -2025-05-21 16:14:24,329 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999986 -0.13500049 0.34416072] -2025-05-21 16:14:24,330 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999818e+02 -8.16753958e-05 -9.00000574e+01] -2025-05-21 16:14:24,332 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999988 -0.13500047 0.34433914] -2025-05-21 16:14:24,336 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -1.43810701e-06 -1.57079726e+00] -2025-05-21 16:14:24,345 - 测试日志 - INFO - log.py:106 - position command: [ 0.2500001 -0.13500026 0.34413976] -2025-05-21 16:14:24,346 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999819e+02 -8.17336472e-05 -8.99999766e+01] -2025-05-21 16:14:24,347 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000012 -0.13500023 0.34433936] -2025-05-21 16:14:24,347 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.43810701e-06 -1.57079579e+00] -2025-05-21 16:14:24,363 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000033 -0.13500038 0.3441226 ] -2025-05-21 16:14:24,363 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999899e+02 -8.17815728e-05 -9.00000086e+01] -2025-05-21 16:14:24,364 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000037 -0.13500035 0.34433997] -2025-05-21 16:14:24,367 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159026e+00 -1.43810701e-06 -1.57079633e+00] -2025-05-21 16:14:24,382 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999973 -0.13500062 0.34410712] -2025-05-21 16:14:24,383 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999743e+02 -5.52633046e-05 -9.00001426e+01] -2025-05-21 16:14:24,386 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000006 -0.13500046 0.34433951] -2025-05-21 16:14:24,393 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.91747555e-06 -1.57079726e+00] -2025-05-21 16:14:24,401 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000001 -0.13500026 0.34409456] -2025-05-21 16:14:24,405 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999669e+02 -3.87423461e-05 -9.00000244e+01] -2025-05-21 16:14:24,411 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000005 -0.13500023 0.34433905] -2025-05-21 16:14:24,413 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -9.58737967e-07 -1.57079633e+00] -2025-05-21 16:14:24,421 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999958 -0.13500027 0.34408394] -2025-05-21 16:14:24,422 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999643e+02 -2.99407130e-05 -9.00000869e+01] -2025-05-21 16:14:24,424 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999992 -0.13500047 0.34433902] -2025-05-21 16:14:24,427 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.43810727e-06 -1.57079726e+00] -2025-05-21 16:14:24,439 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000015 -0.13500338 0.34407621] -2025-05-21 16:14:24,444 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999768e+02 2.11151733e-04 -9.00000910e+01] -2025-05-21 16:14:24,447 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999977 -0.13500035 0.3443392 ] -2025-05-21 16:14:24,451 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -1.43810701e-06 -1.57079686e+00] -2025-05-21 16:14:24,474 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000031 -0.13500502 0.34406939] -2025-05-21 16:14:24,476 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999772e+02 3.30401128e-04 -9.00000107e+01] -2025-05-21 16:14:24,479 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999971 -0.13500047 0.3443392 ] -2025-05-21 16:14:24,481 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -1.43810701e-06 -1.57079726e+00] -2025-05-21 16:14:24,486 - 测试日志 - INFO - log.py:106 - position command: [ 0.2500003 -0.13500534 0.3440657 ] -2025-05-21 16:14:24,487 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999762e+02 3.57333512e-04 -9.00000106e+01] -2025-05-21 16:14:24,489 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999992 -0.13500035 0.34433932] -2025-05-21 16:14:24,495 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -9.58737967e-07 -1.57079686e+00] -2025-05-21 16:14:24,503 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999988 -0.13500529 0.34406178] -2025-05-21 16:14:24,510 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999652e+02 4.03831853e-04 -9.00000685e+01] -2025-05-21 16:14:24,513 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000024 -0.13500035 0.3443399 ] -2025-05-21 16:14:24,516 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159079e+00 -1.43810511e-06 -1.57079726e+00] -2025-05-21 16:14:24,525 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000024 -0.13500502 0.34405848] -2025-05-21 16:14:24,527 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999707e+02 3.22922898e-04 -9.00000528e+01] -2025-05-21 16:14:24,528 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999992 -0.13500035 0.34433868] -2025-05-21 16:14:24,533 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159132e+00 -1.43810765e-06 -1.57079686e+00] -2025-05-21 16:14:24,544 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999997 -0.13500451 0.34405538] -2025-05-21 16:14:24,547 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999732e+02 3.39524808e-04 -9.00000893e+01] -2025-05-21 16:14:24,551 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999995 -0.13500046 0.34433942] -2025-05-21 16:14:24,553 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 -1.91747593e-06 -1.57079633e+00] -2025-05-21 16:14:24,561 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999991 -0.13500429 0.34405252] -2025-05-21 16:14:24,564 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999743e+02 2.43925101e-04 -9.00000610e+01] -2025-05-21 16:14:24,567 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000026 -0.13500047 0.3443389 ] -2025-05-21 16:14:24,570 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.43810727e-06 -1.57079726e+00] -2025-05-21 16:14:24,582 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000021 -0.13500683 0.34404955] -2025-05-21 16:14:24,584 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999635e+02 4.91883119e-04 -9.00001117e+01] -2025-05-21 16:14:24,585 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000021 -0.13500047 0.34433917] -2025-05-21 16:14:24,586 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -1.43810701e-06 -1.57079779e+00] -2025-05-21 16:14:24,600 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000016 -0.13500803 0.34404769] -2025-05-21 16:14:24,602 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999665e+02 5.95358487e-04 -9.00001370e+01] -2025-05-21 16:14:24,604 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000011 -0.13500035 0.34433923] -2025-05-21 16:14:24,606 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -1.43810701e-06 -1.57079779e+00] -2025-05-21 16:14:24,619 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000005 -0.13500826 0.3440459 ] -2025-05-21 16:14:24,621 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999663e+02 6.23875725e-04 -9.00001313e+01] -2025-05-21 16:14:24,623 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000012 -0.13500023 0.34433951] -2025-05-21 16:14:24,627 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 -9.58736634e-07 -1.57079726e+00] -2025-05-21 16:14:24,637 - 测试日志 - INFO - log.py:106 - MassageRobot启动 -2025-05-21 16:14:24,638 - 测试日志 - INFO - log.py:106 - 机械臂控制线程启动 -2025-05-21 16:15:37,544 - 测试日志 - INFO - log.py:106 - 切换到finger_head按摩头 -2025-05-21 16:15:41,053 - 测试日志 - INFO - log.py:106 - 机械臂测量线程启动 -2025-05-21 16:15:41,053 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000002 -0.13499954 0.34428405] -2025-05-21 16:15:41,055 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999939e+02 1.09863358e-04 -9.00000839e+01] -2025-05-21 16:15:41,056 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000029 -0.13499965 0.34433963] -2025-05-21 16:15:41,057 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 1.43810765e-06 -1.57079686e+00] -2025-05-21 16:15:41,062 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000028 -0.13500967 0.34423944] -2025-05-21 16:15:41,063 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999924e+02 1.96828231e-03 -9.00000323e+01] -2025-05-21 16:15:41,065 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999953 -0.13499977 0.34433847] -2025-05-21 16:15:41,066 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159132e+00 1.43810701e-06 -1.57079686e+00] -2025-05-21 16:15:41,078 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000005 -0.13501849 0.3442026 ] -2025-05-21 16:15:41,079 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999861e+02 3.54160759e-03 -9.00000337e+01] -2025-05-21 16:15:41,080 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999968 -0.13499953 0.34433908] -2025-05-21 16:15:41,081 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 1.43810701e-06 -1.57079726e+00] -2025-05-21 16:15:41,096 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999585 -0.13502579 0.34417606] -2025-05-21 16:15:41,098 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999206e+02 4.81360641e-03 -9.00000349e+01] -2025-05-21 16:15:41,101 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.13499977 0.34433917] -2025-05-21 16:15:41,104 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 9.58737777e-07 -1.57079686e+00] -2025-05-21 16:15:41,114 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999399 -0.13502819 0.34415393] -2025-05-21 16:15:41,115 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79998686e+02 5.25549357e-03 -8.99999768e+01] -2025-05-21 16:15:41,117 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999971 -0.13499977 0.34433902] -2025-05-21 16:15:41,125 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 1.43810701e-06 -1.57079779e+00] -2025-05-21 16:15:41,137 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999313 -0.13502822 0.34413444] -2025-05-21 16:15:41,143 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79998493e+02 5.22933991e-03 -9.00001120e+01] -2025-05-21 16:15:41,149 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000002 -0.13499977 0.34433929] -2025-05-21 16:15:41,152 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 4.79368983e-07 -1.57079686e+00] -2025-05-21 16:15:41,161 - 测试日志 - INFO - log.py:106 - position command: [ 0.2499879 -0.13502567 0.34411738] -2025-05-21 16:15:41,163 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79997652e+02 4.79925742e-03 -9.00000365e+01] -2025-05-21 16:15:41,168 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000018 -0.135 0.34433942] -2025-05-21 16:15:41,172 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 1.37845291e-12 -1.57079779e+00] -2025-05-21 16:15:41,177 - 测试日志 - INFO - log.py:106 - position command: [ 0.24998393 -0.13502382 0.34410377] -2025-05-21 16:15:41,181 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79996854e+02 4.44328606e-03 -9.00000888e+01] -2025-05-21 16:15:41,183 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.13499988 0.34433884] -2025-05-21 16:15:41,189 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 4.79368317e-07 -1.57079779e+00] -2025-05-21 16:15:41,198 - 测试日志 - INFO - log.py:106 - position command: [ 0.24998335 -0.13502075 0.34409149] -2025-05-21 16:15:41,205 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79996889e+02 3.98285176e-03 -9.00000879e+01] -2025-05-21 16:15:41,208 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999992 -0.13499989 0.34433917] -2025-05-21 16:15:41,212 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 9.58737777e-07 -1.57079726e+00] -2025-05-21 16:15:41,223 - 测试日志 - INFO - log.py:106 - position command: [ 0.24998388 -0.13501774 0.34408206] -2025-05-21 16:15:41,225 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79996980e+02 3.58493255e-03 -9.00000872e+01] -2025-05-21 16:15:41,227 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000021 -0.13499953 0.34433963] -2025-05-21 16:15:41,228 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159079e+00 9.58739808e-07 -1.57079633e+00] -2025-05-21 16:15:41,236 - 测试日志 - INFO - log.py:106 - position command: [ 0.24998463 -0.13501534 0.34407457] -2025-05-21 16:15:41,241 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79996976e+02 3.18786051e-03 -9.00000054e+01] -2025-05-21 16:15:41,245 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999977 -0.13499965 0.34433887] -2025-05-21 16:15:41,247 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 9.58737522e-07 -1.57079633e+00] -2025-05-21 16:15:41,258 - 测试日志 - INFO - log.py:106 - position command: [ 0.24998601 -0.13501312 0.3440676 ] -2025-05-21 16:15:41,259 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79997343e+02 2.82190161e-03 -8.99999997e+01] -2025-05-21 16:15:41,261 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000008 -0.13499965 0.34433948] -2025-05-21 16:15:41,262 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 9.58738920e-07 -1.57079686e+00] -2025-05-21 16:15:41,275 - 测试日志 - INFO - log.py:106 - position command: [ 0.24998705 -0.13501118 0.34406084] -2025-05-21 16:15:41,277 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79997779e+02 2.46017338e-03 -8.99999969e+01] -2025-05-21 16:15:41,279 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000024 -0.13499977 0.34433975] -2025-05-21 16:15:41,281 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159079e+00 9.58740761e-07 -1.57079779e+00] -2025-05-21 16:15:41,294 - 测试日志 - INFO - log.py:106 - position command: [ 0.24998926 -0.13500915 0.34406031] -2025-05-21 16:15:41,296 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79997937e+02 2.16942173e-03 -8.99999296e+01] -2025-05-21 16:15:41,298 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999994 -0.13499988 0.34433914] -2025-05-21 16:15:41,301 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 4.79368983e-07 -1.57079633e+00] -2025-05-21 16:15:41,314 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999056 -0.13501083 0.34405841] -2025-05-21 16:15:41,318 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79998179e+02 2.38379273e-03 -8.99998972e+01] -2025-05-21 16:15:41,320 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999995 -0.135 0.34433981] -2025-05-21 16:15:41,326 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159079e+00 9.58739808e-07 -1.57079819e+00] -2025-05-21 16:15:41,333 - 测试日志 - INFO - log.py:106 - position command: [ 0.2499923 -0.13501113 0.34405672] -2025-05-21 16:15:41,336 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79998273e+02 2.45611558e-03 -8.99999314e+01] -2025-05-21 16:15:41,342 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000041 -0.13499965 0.34433969] -2025-05-21 16:15:41,344 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159079e+00 1.43810879e-06 -1.57079726e+00] -2025-05-21 16:15:41,353 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999354 -0.13501064 0.34405391] -2025-05-21 16:15:41,355 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79998468e+02 2.42607514e-03 -8.99999611e+01] -2025-05-21 16:15:41,358 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999965 -0.13499977 0.34433868] -2025-05-21 16:15:41,361 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159079e+00 9.58737078e-07 -1.57079686e+00] -2025-05-21 16:15:41,371 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999384 -0.13500999 0.34405066] -2025-05-21 16:15:41,374 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79998809e+02 2.25027449e-03 -8.99999479e+01] -2025-05-21 16:15:41,376 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.13499965 0.34433908] -2025-05-21 16:15:41,379 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 1.43810676e-06 -1.57079686e+00] -2025-05-21 16:15:41,390 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999499 -0.13500937 0.34404941] -2025-05-21 16:15:41,395 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79998809e+02 2.12890937e-03 -8.99999557e+01] -2025-05-21 16:15:41,403 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999994 -0.13499977 0.34433914] -2025-05-21 16:15:41,405 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 9.58737967e-07 -1.57079726e+00] -2025-05-21 16:15:41,415 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999579 -0.13500828 0.34404734] -2025-05-21 16:15:41,420 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79998898e+02 1.95960545e-03 -8.99999885e+01] -2025-05-21 16:15:41,423 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000017 -0.13499977 0.34433911] -2025-05-21 16:15:41,427 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 9.58737967e-07 -1.57079633e+00] -2025-05-21 16:15:41,431 - 测试日志 - INFO - log.py:106 - MassageRobot启动 -2025-05-21 16:15:41,434 - 测试日志 - INFO - log.py:106 - 机械臂控制线程启动 -2025-05-21 16:17:01,723 - 测试日志 - INFO - log.py:106 - 切换到finger_head按摩头 -2025-05-21 16:17:05,236 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000011 -0.13500035 0.34428398] -2025-05-21 16:17:05,239 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.80000000e+02 -8.23974624e-05 -9.00000000e+01] -2025-05-21 16:17:05,241 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999973 -0.13500023 0.34433899] -2025-05-21 16:17:05,242 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.43810701e-06 -1.57079579e+00] -2025-05-21 16:17:05,243 - 测试日志 - INFO - log.py:106 - 机械臂测量线程启动 -2025-05-21 16:17:05,248 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000011 -0.13500035 0.34423891] -2025-05-21 16:17:05,253 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.80000000e+02 -8.23974624e-05 -9.00000000e+01] -2025-05-21 16:17:05,256 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000012 -0.13500023 0.34433951] -2025-05-21 16:17:05,258 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.43810676e-06 -1.57079633e+00] -2025-05-21 16:17:05,264 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000032 -0.13500012 0.34420294] -2025-05-21 16:17:05,267 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999867e+02 -8.22254579e-05 -8.99998990e+01] -2025-05-21 16:17:05,270 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999971 -0.13500023 0.34433835] -2025-05-21 16:17:05,273 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159079e+00 -1.43810701e-06 -1.57079579e+00] -2025-05-21 16:17:05,283 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999972 -0.13500024 0.34417111] -2025-05-21 16:17:05,285 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999834e+02 -8.20813304e-05 -8.99998683e+01] -2025-05-21 16:17:05,288 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000026 -0.13499986 0.34433884] -2025-05-21 16:17:05,291 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.43810701e-06 -1.57079446e+00] -2025-05-21 16:17:05,312 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000019 -0.13500024 0.34414748] -2025-05-21 16:17:05,316 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999919e+02 -5.54241122e-05 -8.99998870e+01] -2025-05-21 16:17:05,323 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999979 -0.13500023 0.34433914] -2025-05-21 16:17:05,326 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.43810701e-06 -1.57079579e+00] -2025-05-21 16:17:05,330 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999995 -0.13500013 0.34412683] -2025-05-21 16:17:05,334 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999761e+02 -5.60960676e-05 -8.99998568e+01] -2025-05-21 16:17:05,336 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999992 -0.13500047 0.34433917] -2025-05-21 16:17:05,337 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.43810727e-06 -1.57079726e+00] -2025-05-21 16:17:05,351 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999994 -0.13500049 0.34411051] -2025-05-21 16:17:05,353 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999734e+02 -8.31957225e-05 -8.99999891e+01] -2025-05-21 16:17:05,355 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999994 -0.13500023 0.34433896] -2025-05-21 16:17:05,358 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -9.58737967e-07 -1.57079633e+00] -2025-05-21 16:17:05,369 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000009 -0.13500013 0.34409743] -2025-05-21 16:17:05,375 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999801e+02 -5.63476622e-05 -8.99999393e+01] -2025-05-21 16:17:05,386 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999965 -0.13500047 0.34433902] -2025-05-21 16:17:05,400 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -1.43810651e-06 -1.57079633e+00] -2025-05-21 16:17:05,403 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000007 -0.13500025 0.34408576] -2025-05-21 16:17:05,407 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999680e+02 -1.14877548e-04 -8.99999571e+01] -2025-05-21 16:17:05,408 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999988 -0.13500023 0.34433896] -2025-05-21 16:17:05,410 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -9.58738475e-07 -1.57079686e+00] -2025-05-21 16:17:05,422 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999989 -0.13500025 0.34407666] -2025-05-21 16:17:05,425 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999605e+02 -1.05207775e-04 -9.00000704e+01] -2025-05-21 16:17:05,429 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000008 -0.135 0.34433963] -2025-05-21 16:17:05,439 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 -4.79368539e-07 -1.57079633e+00] -2025-05-21 16:17:05,443 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999988 -0.13500049 0.34406914] -2025-05-21 16:17:05,445 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999581e+02 -1.40107104e-04 -9.00001422e+01] -2025-05-21 16:17:05,459 - 测试日志 - INFO - log.py:106 - position current:[ 0.2500007 -0.135 0.34433994] -2025-05-21 16:17:05,461 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159079e+00 -9.58737078e-07 -1.57079579e+00] -2025-05-21 16:17:05,463 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000008 -0.13500036 0.34406359] -2025-05-21 16:17:05,465 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999653e+02 -1.41672939e-04 -9.00001497e+01] -2025-05-21 16:17:05,468 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000006 -0.13500035 0.34433936] -2025-05-21 16:17:05,469 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.43810701e-06 -1.57079633e+00] -2025-05-21 16:17:05,483 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000016 -0.1350007 0.34405844] -2025-05-21 16:17:05,484 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999640e+02 -2.37568578e-04 -9.00002347e+01] -2025-05-21 16:17:05,486 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000034 -0.135 0.34433917] -2025-05-21 16:17:05,495 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -4.79368983e-07 -1.57079633e+00] -2025-05-21 16:17:05,504 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000024 -0.13500023 0.34405465] -2025-05-21 16:17:05,505 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999684e+02 -2.40440778e-04 -9.00001249e+01] -2025-05-21 16:17:05,507 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999988 -0.13500035 0.34433881] -2025-05-21 16:17:05,509 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -1.43810651e-06 -1.57079579e+00] -2025-05-21 16:17:05,523 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000033 -0.13500024 0.34405139] -2025-05-21 16:17:05,525 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999739e+02 -2.66054737e-04 -9.00000777e+01] -2025-05-21 16:17:05,526 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000011 -0.13500035 0.34433957] -2025-05-21 16:17:05,528 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 -9.58738475e-07 -1.57079633e+00] -2025-05-21 16:17:05,543 - 测试日志 - INFO - log.py:106 - position command: [ 0.25 -0.13500014 0.34404819] -2025-05-21 16:17:05,545 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999680e+02 -2.44633239e-04 -9.00000740e+01] -2025-05-21 16:17:05,549 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999989 -0.13500023 0.34433926] -2025-05-21 16:17:05,558 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -9.58737967e-07 -1.57079633e+00] -2025-05-21 16:17:05,561 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000033 -0.13500048 0.34404618] -2025-05-21 16:17:05,562 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999773e+02 -2.91165006e-04 -9.00000499e+01] -2025-05-21 16:17:05,564 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000034 -0.13500047 0.34433923] -2025-05-21 16:17:05,572 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.43810727e-06 -1.57079633e+00] -2025-05-21 16:17:05,579 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999978 -0.13500024 0.34404383] -2025-05-21 16:17:05,587 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999715e+02 -2.75846174e-04 -9.00000035e+01] -2025-05-21 16:17:05,590 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000023 -0.13500035 0.34433945] -2025-05-21 16:17:05,592 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.43810701e-06 -1.57079633e+00] -2025-05-21 16:17:05,597 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000048 -0.13499978 0.34404342] -2025-05-21 16:17:05,604 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999838e+02 -2.04032016e-04 -8.99999410e+01] -2025-05-21 16:17:05,607 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000012 -0.13500023 0.34433936] -2025-05-21 16:17:05,612 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.43810701e-06 -1.57079579e+00] -2025-05-21 16:17:05,618 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000012 -0.13500026 0.34404177] -2025-05-21 16:17:05,625 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999796e+02 -2.38866916e-04 -8.99999868e+01] -2025-05-21 16:17:05,628 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000024 -0.135 0.34433908] -2025-05-21 16:17:05,633 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 -9.58737967e-07 -1.57079486e+00] -2025-05-21 16:17:05,635 - 测试日志 - INFO - log.py:106 - 机械臂控制线程启动 -2025-05-21 16:17:05,639 - 测试日志 - INFO - log.py:106 - MassageRobot启动 -2025-05-21 16:18:44,757 - 测试日志 - INFO - log.py:106 - 切换到finger_head按摩头 -2025-05-21 16:18:48,266 - 测试日志 - INFO - log.py:106 - 机械臂测量线程启动 -2025-05-21 16:18:48,267 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999995 -0.135 0.34428365] -2025-05-21 16:18:48,269 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999969e+02 -5.49316937e-05 -9.00000534e+01] -2025-05-21 16:18:48,270 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000031 -0.13499988 0.34433939] -2025-05-21 16:18:48,271 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265 0. -1.57079726] -2025-05-21 16:18:48,275 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000029 -0.13499988 0.34423919] -2025-05-21 16:18:48,277 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999999e+02 -1.81592375e-06 -9.00000534e+01] -2025-05-21 16:18:48,278 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000047 -0.13499988 0.34433951] -2025-05-21 16:18:48,279 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 -4.79368063e-07 -1.57079633e+00] -2025-05-21 16:18:48,291 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000045 -0.13499988 0.34420241] -2025-05-21 16:18:48,291 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999865e+02 -2.98283943e-05 -8.99999152e+01] -2025-05-21 16:18:48,293 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000037 -0.13499965 0.34433957] -2025-05-21 16:18:48,293 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159079e+00 4.79372666e-07 -1.57079726e+00] -2025-05-21 16:18:48,310 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000034 -0.13499966 0.34417228] -2025-05-21 16:18:48,324 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999724e+02 8.75388511e-05 -8.99998653e+01] -2025-05-21 16:18:48,326 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000002 -0.13499988 0.34433929] -2025-05-21 16:18:48,327 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 4.79369460e-07 -1.57079686e+00] -2025-05-21 16:18:48,329 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000033 -0.13499967 0.34414757] -2025-05-21 16:18:48,340 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999646e+02 1.38622327e-04 -8.99997808e+01] -2025-05-21 16:18:48,342 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999977 -0.13500012 0.34433875] -2025-05-21 16:18:48,344 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -6.89004409e-13 -1.57079819e+00] -2025-05-21 16:18:48,350 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999998 -0.1349999 0.34412709] -2025-05-21 16:18:48,352 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999626e+02 1.80417906e-04 -8.99996895e+01] -2025-05-21 16:18:48,354 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999968 -0.13499977 0.34433893] -2025-05-21 16:18:48,355 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 4.79368983e-07 -1.57079726e+00] -2025-05-21 16:18:48,369 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999966 -0.1349998 0.34411021] -2025-05-21 16:18:48,371 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999691e+02 2.70855426e-04 -8.99996277e+01] -2025-05-21 16:18:48,371 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.13500012 0.34433926] -2025-05-21 16:18:48,372 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 6.89004409e-13 -1.57079819e+00] -2025-05-21 16:18:48,385 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999993 -0.1349999 0.34409657] -2025-05-21 16:18:48,386 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999720e+02 2.83913412e-04 -8.99995511e+01] -2025-05-21 16:18:48,389 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999973 -0.13499988 0.34433899] -2025-05-21 16:18:48,390 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -6.89226454e-13 -1.57079726e+00] -2025-05-21 16:18:48,403 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999972 -0.1349999 0.3440857 ] -2025-05-21 16:18:48,405 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999688e+02 2.93034070e-04 -8.99994834e+01] -2025-05-21 16:18:48,407 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000017 -0.13499988 0.34433939] -2025-05-21 16:18:48,409 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265 0. -1.57079726] -2025-05-21 16:18:48,427 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999993 -0.13500002 0.34407679] -2025-05-21 16:18:48,429 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999675e+02 2.83350090e-04 -8.99994549e+01] -2025-05-21 16:18:48,433 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.135 0.34433908] -2025-05-21 16:18:48,437 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -6.89004409e-13 -1.57079779e+00] -2025-05-21 16:18:48,443 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999993 -0.13500002 0.34406938] -2025-05-21 16:18:48,444 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999702e+02 2.16186950e-04 -8.99994031e+01] -2025-05-21 16:18:48,445 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000006 -0.13499988 0.34433917] -2025-05-21 16:18:48,447 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 4.79369904e-07 -1.57079819e+00] -2025-05-21 16:18:48,462 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000005 -0.13500001 0.34406353] -2025-05-21 16:18:48,464 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999703e+02 2.11693330e-04 -8.99993587e+01] -2025-05-21 16:18:48,468 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000017 -0.13499977 0.34433908] -2025-05-21 16:18:48,471 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 4.79368983e-07 -1.57079779e+00] -2025-05-21 16:18:48,478 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000015 -0.13499979 0.3440583 ] -2025-05-21 16:18:48,480 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999765e+02 1.78019813e-04 -8.99993211e+01] -2025-05-21 16:18:48,482 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999985 -0.13499977 0.34433887] -2025-05-21 16:18:48,484 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 4.79368983e-07 -1.57079779e+00] -2025-05-21 16:18:48,499 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999984 -0.13499979 0.34405403] -2025-05-21 16:18:48,502 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999796e+02 1.79938874e-04 -8.99993358e+01] -2025-05-21 16:18:48,504 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999985 -0.13499988 0.34433936] -2025-05-21 16:18:48,506 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265 0. -1.57079726] -2025-05-21 16:18:48,528 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000001 -0.1349998 0.34405081] -2025-05-21 16:18:48,528 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999827e+02 1.53956350e-04 -8.99993294e+01] -2025-05-21 16:18:48,529 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000002 -0.13499977 0.34433896] -2025-05-21 16:18:48,530 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159265e+00 4.79368983e-07 -1.57079726e+00] -2025-05-21 16:18:48,548 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000002 -0.1349999 0.34404839] -2025-05-21 16:18:48,552 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999823e+02 7.95818872e-05 -8.99993785e+01] -2025-05-21 16:18:48,553 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000031 -0.13499976 0.34433942] -2025-05-21 16:18:48,553 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 1.83830728e-12 -1.57079726e+00] -2025-05-21 16:18:48,565 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000023 -0.13499956 0.34404566] -2025-05-21 16:18:48,568 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999935e+02 1.62127130e-04 -8.99993259e+01] -2025-05-21 16:18:48,569 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000008 -0.13499988 0.34433948] -2025-05-21 16:18:48,570 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 1.83808524e-12 -1.57079779e+00] -2025-05-21 16:18:48,584 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000006 -0.13499991 0.34404457] -2025-05-21 16:18:48,585 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999790e+02 1.34871105e-04 -8.99993576e+01] -2025-05-21 16:18:48,587 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999985 -0.13499988 0.34433868] -2025-05-21 16:18:48,588 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -4.79369904e-07 -1.57079633e+00] -2025-05-21 16:18:48,602 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000027 -0.13499991 0.34404304] -2025-05-21 16:18:48,604 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999814e+02 1.74399647e-04 -8.99993335e+01] -2025-05-21 16:18:48,604 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000026 -0.13499976 0.34433951] -2025-05-21 16:18:48,605 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 3.44657636e-12 -1.57079779e+00] -2025-05-21 16:18:48,622 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999991 -0.13500014 0.34404122] -2025-05-21 16:18:48,624 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999830e+02 1.88944179e-04 -8.99993904e+01] -2025-05-21 16:18:48,626 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999977 -0.13499977 0.34433868] -2025-05-21 16:18:48,629 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 4.79367142e-07 -1.57079779e+00] -2025-05-21 16:18:48,639 - 测试日志 - INFO - log.py:106 - MassageRobot启动 -2025-05-21 16:18:48,639 - 测试日志 - INFO - log.py:106 - 机械臂控制线程启动 +2025-05-26 11:54:18,076 - 测试日志 - INFO - log.py:106 - 切换到finger_head按摩头 +2025-05-26 11:54:21,584 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999992 -0.13500023 0.34428392] +2025-05-26 11:54:21,586 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999969e+02 -2.74658596e-05 -9.00001068e+01] +2025-05-26 11:54:21,586 - 测试日志 - INFO - log.py:106 - 机械臂测量线程启动 +2025-05-26 11:54:21,587 - 测试日志 - INFO - log.py:106 - position current:[ 0.2499996 -0.13500023 0.34433881] +2025-05-26 11:54:21,588 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -4.79370158e-07 -1.57079926e+00] +2025-05-26 11:54:21,589 - 测试日志 - INFO - log.py:106 - position command: [ 0.24999961 -0.13500023 0.34423864] +2025-05-26 11:54:21,590 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999969e+02 -2.74658860e-05 -9.00001658e+01] +2025-05-26 11:54:21,591 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999948 -0.13500035 0.34433908] +2025-05-26 11:54:21,592 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -4.79368983e-07 -1.57079872e+00] +2025-05-26 11:54:21,608 - 测试日志 - INFO - log.py:106 - position command: [ 0.24998909 -0.13500023 0.34420176] +2025-05-26 11:54:21,609 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79997496e+02 -2.74133618e-05 -9.00001642e+01] +2025-05-26 11:54:21,610 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000005 -0.13500023 0.34433908] +2025-05-26 11:54:21,612 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -4.79368984e-07 -1.57079819e+00] +2025-05-26 11:54:21,625 - 测试日志 - INFO - log.py:106 - position command: [ 0.24998073 -0.13500023 0.34417185] +2025-05-26 11:54:21,626 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79995349e+02 -2.73680173e-05 -9.00001038e+01] +2025-05-26 11:54:21,627 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999977 -0.13500035 0.34433926] +2025-05-26 11:54:21,628 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -4.79368063e-07 -1.57079926e+00] +2025-05-26 11:54:21,643 - 测试日志 - INFO - log.py:106 - position command: [ 0.24997316 -0.13500035 0.34414734] +2025-05-26 11:54:21,644 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79993563e+02 -2.73309329e-05 -9.00001634e+01] +2025-05-26 11:54:21,647 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999969 -0.13500024 0.34433875] +2025-05-26 11:54:21,649 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -6.89226454e-13 -1.57079872e+00] +2025-05-26 11:54:21,663 - 测试日志 - INFO - log.py:106 - position command: [ 0.24996706 -0.13500024 0.34412663] +2025-05-26 11:54:21,666 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79992223e+02 -7.61538884e-07 -9.00001327e+01] +2025-05-26 11:54:21,668 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000006 -0.135 0.34433902] +2025-05-26 11:54:21,669 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -4.79369904e-07 -1.57079779e+00] +2025-05-26 11:54:21,681 - 测试日志 - INFO - log.py:106 - position command: [ 0.24996206 -0.13500023 0.34411067] +2025-05-26 11:54:21,683 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79990992e+02 -2.80462379e-05 -9.00001030e+01] +2025-05-26 11:54:21,684 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000006 -0.135 0.34433936] +2025-05-26 11:54:21,684 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 9.19486709e-13 -1.57079819e+00] +2025-05-26 11:54:21,699 - 测试日志 - INFO - log.py:106 - position command: [ 0.24995834 -0.13500001 0.34409717] +2025-05-26 11:54:21,699 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79990018e+02 -1.35795159e-06 -9.00000559e+01] +2025-05-26 11:54:21,700 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000023 -0.13500023 0.34433963] +2025-05-26 11:54:21,700 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 -4.79366253e-07 -1.57079872e+00] +2025-05-26 11:54:21,720 - 测试日志 - INFO - log.py:106 - position command: [ 0.24995512 -0.13500023 0.34408636] +2025-05-26 11:54:21,723 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79989165e+02 -2.85437621e-05 -9.00000600e+01] +2025-05-26 11:54:21,724 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999965 -0.13500023 0.34433887] +2025-05-26 11:54:21,726 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -4.79370380e-07 -1.57079819e+00] +2025-05-26 11:54:21,735 - 测试日志 - INFO - log.py:106 - position command: [ 0.24995177 -0.13500013 0.34407655] +2025-05-26 11:54:21,736 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79988628e+02 -1.76782008e-06 -9.00000183e+01] +2025-05-26 11:54:21,737 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000005 -0.13500023 0.34433908] +2025-05-26 11:54:21,738 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -4.79368983e-07 -1.57079779e+00] +2025-05-26 11:54:21,753 - 测试日志 - INFO - log.py:106 - position command: [ 0.24994988 -0.13500023 0.34406936] +2025-05-26 11:54:21,754 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79988058e+02 -2.88798869e-05 -8.99999938e+01] +2025-05-26 11:54:21,755 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999965 -0.13500024 0.3443392 ] +2025-05-26 11:54:21,755 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265 0. -1.57079819] +2025-05-26 11:54:21,771 - 测试日志 - INFO - log.py:106 - position command: [ 0.24994762 -0.13500025 0.34406342] +2025-05-26 11:54:21,772 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79987624e+02 -2.04317933e-06 -9.00000197e+01] +2025-05-26 11:54:21,772 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.13500012 0.34433936] +2025-05-26 11:54:21,773 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 9.19042620e-13 -1.57079872e+00] +2025-05-26 11:54:21,790 - 测试日志 - INFO - log.py:106 - position command: [ 0.24994642 -0.13500013 0.34405861] +2025-05-26 11:54:21,790 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79987227e+02 -2.56610781e-06 -9.00000552e+01] +2025-05-26 11:54:21,791 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999992 -0.13500023 0.34433884] +2025-05-26 11:54:21,792 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -4.79369904e-07 -1.57079872e+00] +2025-05-26 11:54:21,808 - 测试日志 - INFO - log.py:106 - position command: [ 0.24994508 -0.13500023 0.34405405] +2025-05-26 11:54:21,809 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79987023e+02 -2.95401718e-05 -9.00000620e+01] +2025-05-26 11:54:21,809 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999988 -0.13500012 0.34433881] +2025-05-26 11:54:21,810 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -4.79369904e-07 -1.57079726e+00] +2025-05-26 11:54:21,826 - 测试日志 - INFO - log.py:106 - position command: [ 0.24994399 -0.13500013 0.34405071] +2025-05-26 11:54:21,827 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79986863e+02 -2.91276232e-05 -8.99999883e+01] +2025-05-26 11:54:21,828 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999983 -0.13500012 0.34433908] +2025-05-26 11:54:21,829 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -4.79369460e-07 -1.57079726e+00] +2025-05-26 11:54:21,845 - 测试日志 - INFO - log.py:106 - position command: [ 0.24994314 -0.13500035 0.34404815] +2025-05-26 11:54:21,846 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79986622e+02 -5.53243140e-05 -9.00000498e+01] +2025-05-26 11:54:21,847 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000023 -0.13500023 0.34433929] +2025-05-26 11:54:21,849 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 -4.79367618e-07 -1.57079779e+00] +2025-05-26 11:54:21,863 - 测试日志 - INFO - log.py:106 - position command: [ 0.24994279 -0.13500023 0.34404625] +2025-05-26 11:54:21,864 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79986428e+02 -2.77276668e-05 -9.00000352e+01] +2025-05-26 11:54:21,865 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999953 -0.13500024 0.34433899] +2025-05-26 11:54:21,866 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -6.89226454e-13 -1.57079872e+00] +2025-05-26 11:54:21,881 - 测试日志 - INFO - log.py:106 - position command: [ 0.24994152 -0.13500025 0.34404413] +2025-05-26 11:54:21,883 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79986454e+02 -1.08669986e-06 -9.00000479e+01] +2025-05-26 11:54:21,883 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000006 -0.13500023 0.34433936] +2025-05-26 11:54:21,884 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 -4.79366920e-07 -1.57079819e+00] +2025-05-26 11:54:21,900 - 测试日志 - INFO - log.py:106 - position command: [ 0.24994158 -0.13500023 0.344043 ] +2025-05-26 11:54:21,901 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79986300e+02 -2.83187191e-05 -8.99999517e+01] +2025-05-26 11:54:21,902 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000012 -0.135 0.34433951] +2025-05-26 11:54:21,903 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 2.75734990e-12 -1.57079819e+00] +2025-05-26 11:54:21,918 - 测试日志 - INFO - log.py:106 - position command: [ 0.24994125 -0.13500001 0.34404193] +2025-05-26 11:54:21,919 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79986271e+02 -1.58659370e-06 -8.99998736e+01] +2025-05-26 11:54:21,919 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000011 -0.13500012 0.34433923] +2025-05-26 11:54:21,919 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 1.83808524e-12 -1.57079872e+00] +2025-05-26 11:54:21,936 - 测试日志 - INFO - log.py:106 - MassageRobot启动 +2025-05-26 11:54:21,937 - 测试日志 - INFO - log.py:106 - 机械臂控制线程启动 +2025-05-26 11:55:40,378 - 测试日志 - INFO - log.py:106 - 切换到finger_head按摩头 +2025-05-26 11:55:43,889 - 测试日志 - INFO - log.py:106 - 机械臂测量线程启动 +2025-05-26 11:55:43,891 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000014 -0.135 0.34428438] +2025-05-26 11:55:43,894 - 测试日志 - INFO - log.py:106 - orientation command: [-1.79999939e+02 -5.49315882e-05 -9.00000000e+01] +2025-05-26 11:55:43,898 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000023 -0.13500023 0.34433896] +2025-05-26 11:55:43,900 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.43810765e-06 -1.57079726e+00] +2025-05-26 11:55:43,902 - 测试日志 - INFO - log.py:106 - position command: [ 0.25001025 -0.13501025 0.3442388 ] +2025-05-26 11:55:43,902 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79997559e+02 2.17593076e-03 -9.00002084e+01] +2025-05-26 11:55:43,903 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000023 -0.13500035 0.34433945] +2025-05-26 11:55:43,903 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -1.43810676e-06 -1.57079686e+00] +2025-05-26 11:55:43,920 - 测试日志 - INFO - log.py:106 - position command: [ 0.25001334 -0.13501341 0.3442019 ] +2025-05-26 11:55:43,921 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79996755e+02 2.88947329e-03 -9.00002564e+01] +2025-05-26 11:55:43,922 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000018 -0.13499998 0.34433917] +2025-05-26 11:55:43,922 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.43810739e-06 -1.57079579e+00] +2025-05-26 11:55:43,938 - 测试日志 - INFO - log.py:106 - position command: [ 0.25001398 -0.13501412 0.34417227] +2025-05-26 11:55:43,938 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79996719e+02 3.05105884e-03 -9.00002148e+01] +2025-05-26 11:55:43,939 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999994 -0.13500023 0.34433881] +2025-05-26 11:55:43,940 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -1.43810701e-06 -1.57079579e+00] +2025-05-26 11:55:43,956 - 测试日志 - INFO - log.py:106 - position command: [ 0.25001377 -0.13501382 0.34414739] +2025-05-26 11:55:43,957 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79996789e+02 2.93070895e-03 -9.00002087e+01] +2025-05-26 11:55:43,957 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000031 -0.13499988 0.34433939] +2025-05-26 11:55:43,958 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -9.58737967e-07 -1.57079579e+00] +2025-05-26 11:55:43,974 - 测试日志 - INFO - log.py:106 - position command: [ 0.25001229 -0.13501272 0.3441266 ] +2025-05-26 11:55:43,975 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79996934e+02 2.74026705e-03 -9.00001939e+01] +2025-05-26 11:55:43,976 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999977 -0.13500023 0.34433868] +2025-05-26 11:55:43,977 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -1.43810739e-06 -1.57079633e+00] +2025-05-26 11:55:43,993 - 测试日志 - INFO - log.py:106 - position command: [ 0.25001103 -0.13501121 0.34411053] +2025-05-26 11:55:43,995 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79997359e+02 2.40078952e-03 -9.00002833e+01] +2025-05-26 11:55:43,996 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000002 -0.13500023 0.34433929] +2025-05-26 11:55:43,997 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -1.43810701e-06 -1.57079579e+00] +2025-05-26 11:55:44,012 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000991 -0.13501245 0.34409683] +2025-05-26 11:55:44,013 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79997725e+02 2.82936110e-03 -9.00002243e+01] +2025-05-26 11:55:44,014 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000023 -0.13500011 0.34433896] +2025-05-26 11:55:44,015 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -9.58738221e-07 -1.57079633e+00] +2025-05-26 11:55:44,032 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000804 -0.13501408 0.34408536] +2025-05-26 11:55:44,034 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79997975e+02 3.15753842e-03 -9.00003258e+01] +2025-05-26 11:55:44,035 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999989 -0.135 0.34433923] +2025-05-26 11:55:44,036 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -4.79369237e-07 -1.57079633e+00] +2025-05-26 11:55:44,048 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000729 -0.13501416 0.34407724] +2025-05-26 11:55:44,050 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79998429e+02 3.19397557e-03 -9.00003213e+01] +2025-05-26 11:55:44,052 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000027 -0.13500011 0.34433899] +2025-05-26 11:55:44,053 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -9.58738475e-07 -1.57079633e+00] +2025-05-26 11:55:44,067 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000643 -0.13501629 0.34406971] +2025-05-26 11:55:44,069 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79998650e+02 3.77513833e-03 -9.00002451e+01] +2025-05-26 11:55:44,072 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999985 -0.135 0.34433887] +2025-05-26 11:55:44,073 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -9.58738919e-07 -1.57079633e+00] +2025-05-26 11:55:44,085 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000533 -0.13501711 0.34406281] +2025-05-26 11:55:44,086 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79998765e+02 3.90683349e-03 -9.00002227e+01] +2025-05-26 11:55:44,087 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.13500046 0.34433902] +2025-05-26 11:55:44,088 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.91747644e-06 -1.57079726e+00] +2025-05-26 11:55:44,104 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000444 -0.1350157 0.34405811] +2025-05-26 11:55:44,105 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79998963e+02 3.63745661e-03 -9.00001697e+01] +2025-05-26 11:55:44,106 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000018 -0.13500023 0.34433942] +2025-05-26 11:55:44,107 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159159e+00 -9.58738475e-07 -1.57079579e+00] +2025-05-26 11:55:44,123 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000359 -0.13501458 0.34405421] +2025-05-26 11:55:44,124 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999129e+02 3.26760004e-03 -9.00001930e+01] +2025-05-26 11:55:44,125 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999994 -0.13500023 0.34433896] +2025-05-26 11:55:44,126 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159159e+00 -1.43810790e-06 -1.57079686e+00] +2025-05-26 11:55:44,142 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000288 -0.13501325 0.34405068] +2025-05-26 11:55:44,143 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999280e+02 3.04057788e-03 -9.00001978e+01] +2025-05-26 11:55:44,144 - 测试日志 - INFO - log.py:106 - position current:[ 0.25 -0.13500035 0.34433917] +2025-05-26 11:55:44,145 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.43810727e-06 -1.57079686e+00] +2025-05-26 11:55:44,159 - 测试日志 - INFO - log.py:106 - position command: [ 0.2500026 -0.13501201 0.34404842] +2025-05-26 11:55:44,161 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999500e+02 2.69362248e-03 -9.00001960e+01] +2025-05-26 11:55:44,162 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000017 -0.13500011 0.34433939] +2025-05-26 11:55:44,163 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159265e+00 -9.58737967e-07 -1.57079633e+00] +2025-05-26 11:55:44,177 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000177 -0.13501061 0.34404554] +2025-05-26 11:55:44,178 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999487e+02 2.36677523e-03 -9.00001222e+01] +2025-05-26 11:55:44,180 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999989 -0.13500023 0.34433908] +2025-05-26 11:55:44,182 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -1.43810701e-06 -1.57079579e+00] +2025-05-26 11:55:44,195 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000221 -0.13500882 0.34404453] +2025-05-26 11:55:44,196 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999744e+02 2.10471007e-03 -9.00000524e+01] +2025-05-26 11:55:44,197 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000012 -0.13500011 0.34433936] +2025-05-26 11:55:44,199 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159212e+00 -9.58737967e-07 -1.57079579e+00] +2025-05-26 11:55:44,214 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000156 -0.13501309 0.34404321] +2025-05-26 11:55:44,215 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999837e+02 2.97172045e-03 -9.00001404e+01] +2025-05-26 11:55:44,216 - 测试日志 - INFO - log.py:106 - position current:[ 0.25000009 -0.13500011 0.34433942] +2025-05-26 11:55:44,217 - 测试日志 - INFO - log.py:106 - orientation current: [-3.14159132e+00 -1.43810562e-06 -1.57079633e+00] +2025-05-26 11:55:44,232 - 测试日志 - INFO - log.py:106 - position command: [ 0.25000442 -0.1350145 0.3440416 ] +2025-05-26 11:55:44,233 - 测试日志 - INFO - log.py:106 - orientation command: [ 1.79999110e+02 3.39907171e-03 -9.00001022e+01] +2025-05-26 11:55:44,234 - 测试日志 - INFO - log.py:106 - position current:[ 0.24999973 -0.13500011 0.34433881] +2025-05-26 11:55:44,235 - 测试日志 - INFO - log.py:106 - orientation current: [ 3.14159212e+00 -9.58738221e-07 -1.57079633e+00] +2025-05-26 11:55:44,253 - 测试日志 - INFO - log.py:106 - 机械臂控制线程启动 +2025-05-26 11:55:44,253 - 测试日志 - INFO - log.py:106 - MassageRobot启动