stash

Massage/Massage.py

@@ -1990,6 +1990,7 @@ class TaskBase(smach.State):
             use_algorithm = self.resources.use_algorithm
             if self.resources.choose_task != "ion" and self.resources.motion_task[5] == "point":
                 self.resources.use_algorithm = "hybridPid"
+                print("self.resources.use_algorithm:",self.resources.use_algorithm)
             code = self.resources.robot.move_to_point(pose=self.resources.target_points, t=self.resources.motion_task[6], timeout=0.5, interpolation="circle", algorithm=self.resources.use_algorithm,is_interrupt=False,is_switch_controller = self.resources.is_switch_controller)
             self.resources.use_algorithm = use_algorithm
         else:

Massage/MassageControl/MassageRobot_nova5.py

@@ -74,6 +74,8 @@ except:
 import matplotlib.pyplot as plt
 from matplotlib.animation import FuncAnimation 
 
+import select
+
 current_file_path = os.path.abspath(__file__)
 MassageRobot_Dobot_Path = os.path.dirname(os.path.dirname(os.path.dirname(current_file_path)))
 print(MassageRobot_Dobot_Path)
@@ -165,17 +167,17 @@ class MassageRobot:
         self.adjust_wrench = np.zeros(6,dtype=np.float64)
         self.skip_pos = np.zeros(6,dtype=np.float64)
 
-        #按摩头参数暂时使用本地数据
-        # massage_head_dir = self.arm_config['massage_head_dir']
-        # all_items = os.listdir(massage_head_dir)
-        # head_config_files = [f for f in all_items if os.path.isfile(os.path.join(massage_head_dir, f))]
-        # self.playload_dict = {}
-        # for file in head_config_files:
-        #     file_address = massage_head_dir + '/' + file
-        #     play_load = read_yaml(file_address)
-        #     self.playload_dict[play_load['name']] = play_load
+        # 按摩头参数暂时使用本地数据
+        massage_head_dir = self.arm_config['massage_head_dir']
+        all_items = os.listdir(massage_head_dir)
+        head_config_files = [f for f in all_items if os.path.isfile(os.path.join(massage_head_dir, f))]
+        self.playload_dict = {}
+        for file in head_config_files:
+            file_address = massage_head_dir + '/' + file
+            play_load = read_yaml(file_address)
+            self.playload_dict[play_load['name']] = play_load
 
-        self.playload_dict = self.vtxdb.get("robot_config", "massage_head")
+        # self.playload_dict = self.vtxdb.get("robot_config", "massage_head")
         # print(self.playload_dict)
         self.current_head = 'none'
         self.is_waitting = False
@@ -196,9 +198,13 @@ class MassageRobot:
         self.x_tcp = np.zeros(6)
         self.P_tcp  = np.eye(6)
         # 过程噪声协方差矩阵
-        self.Q_tcp = np.eye(6) * 0.01
+        # self.Q_tcp = np.eye(6) * 0.01
+        self.Q_tcp = np.eye(6) * 0.05
+
         # 测量噪声协方差矩阵
-        self.R_tcp = np.eye(6) * 0.1
+        # self.R_tcp = np.eye(6) * 0.1
+        self.R_tcp = np.diag([0.5, 0.5, 0.5, 0.05, 0.05, 0.05])
+
         # 传感器故障计数器
         self.sensor_fail_count = 0 
         # 机械臂初始化,适配中间层
@@ -421,7 +427,8 @@ class MassageRobot:
             self.logger.log_blue(f"current position and orientation:{self.arm_state.arm_position},{self.arm_state.arm_orientation}")
             self.logger.log_yellow("---------------------change----------------------")
             self.arm.change_end_effector(self.playload_dict[name])
-            self.logger.log_info(f"切换到{name}按摩头")
+
+            self.logger.log_info(f"切换到{name}按摩头,{self.arm.tcp_offset}")
             R_matrix = R.from_euler('xyz',self.ready_pose[3:] ,degrees=False).as_matrix()
             ready_position = self.ready_pose[:3] + R_matrix @ self.playload_dict[self.current_head]['tcp_offset'][:3]
             ready_orientation =  R_matrix @ R.from_euler('xyz',self.playload_dict[self.current_head]['tcp_offset'][3:],degrees=False).as_matrix()
@@ -563,7 +570,7 @@ class MassageRobot:
                     pose_processed[:3] = temp_pose[:3] - target_matrix @ np.array(self.arm.tcp_offset[:3])*1000
                     pose_processed[3:] = R.from_matrix(target_matrix).as_euler("xyz",degrees = True)
                     
-                    print(f"pose_processed:{pose_processed},{self.arm_state.arm_position_command * 1000}")  
+                    # print(f"pose_processed:{pose_processed},{self.arm_state.arm_position_command * 1000}")  
                     
                     tcp_command = (
                     f"ServoP({pose_processed[0]:.3f},{pose_processed[1]:.3f},"
@@ -647,11 +654,11 @@ class MassageRobot:
             self.arm_state.arm_orientation = quat_rot
             for i in range(20):
                 self.controller_manager.step(self.control_rate.to_sec())
-                self.logger.log_blue(f"position command: {self.arm_state.arm_position_command}")
-                self.logger.log_blue(f"orientation command: {R.from_quat(self.arm_state.arm_orientation_command).as_euler('xyz',degrees=True)}")
+                # self.logger.log_blue(f"position command: {self.arm_state.arm_position_command}")
+                # self.logger.log_blue(f"orientation command: {R.from_quat(self.arm_state.arm_orientation_command).as_euler('xyz',degrees=True)}")
                 position, quat_rot = self.arm.get_arm_position()
-                self.logger.log_blue(f"position current:{position}")
-                self.logger.log_blue(f"orientation current: {R.from_quat(quat_rot).as_euler('xyz',degrees=False)}")
+                # self.logger.log_blue(f"position current:{position}")
+                # self.logger.log_blue(f"orientation current: {R.from_quat(quat_rot).as_euler('xyz',degrees=False)}")
                 # print("self.arm.feedbackData.robotMode:",self.arm.feedbackData.robotMode)
                 self.command_rate.precise_sleep()
                 
@@ -678,6 +685,9 @@ class MassageRobot:
             self.interrupt_event.clear()
             self.arm_control_thread.join()
             self.arm_measure_thread.join()
+            # clear_time = time.time()
+            # self.clear_socket_buffer(self.arm.dashboard.socket_dobot)
+            # print(f"==={time.time()-clear_time}===")
 
     def stop(self):
         for i in range(3):
@@ -691,6 +701,21 @@ class MassageRobot:
         # else:
         #     self.logger.log_error(f"未找到{name}按摩头")
 
+    # def clear_socket_buffer(self, sock):
+    #     # 使用 select 检查 socket 是否有数据
+    #     print("clear_socket_buffer")
+    #     while True:
+    #         ready, _, _ = select.select([sock], [], [], 0)
+    #         if ready:
+    #             try:
+    #                 data = sock.recv(4096)
+    #                 if not data:
+    #                     break  # 对方关闭连接
+    #             except BlockingIOError:
+    #                 break
+    #         else:
+    #             print("socket_buffer EMPTY")
+    #             break  # 没有更多数据了
 
     # def stop(self):
     #     if not self.exit_event.is_set():
@@ -1535,7 +1560,7 @@ class MassageRobot:
                     pos_traj, quat_traj = spline_interpolate(positions=pose_matrix[:, :3], quaternions=pose_matrix[:, 3:], time_points=time_points, time_step=dt)
                 elif interpolation == 'circle':
                     if self.ion is not None:
-                        pos_traj = circle_trajectory(center=pose, radius=0.07,time_points=time_points,time_step=dt,**kwargs)
+                        pos_traj = circle_trajectory(center=pose, radius=0.09,time_points=time_points,time_step=dt,**kwargs)
                     else:
                         pos_traj = circle_trajectory(center=pose,time_points=time_points,time_step=dt,**kwargs)
                     quat_traj = np.tile(R.from_euler('xyz', np.array(pose[0][3:])).as_quat(), (pos_traj.shape[0], 1))
@@ -1771,11 +1796,12 @@ if __name__ == "__main__":
     robot.force_sensor.set_zero()
     time.sleep(0.5)
     robot.force_sensor.enable_active_transmission()
-    robot.set_position(pose = ready_pose)
+    # robot.set_position(pose = ready_pose)
     
     robot.init_hardwares(ready_pose)
     time.sleep(3)
     robot.switch_payload("finger_head")
+    print("111111111111111111111111111111111111111111111111111111111111111111111111111111")
     robot.controller_manager.switch_controller('admittance')
     # position,quat_rot = robot.arm.get_arm_position()
     # euler_rot = R.from_quat(quat_rot).as_euler('xyz')
@@ -1787,9 +1813,14 @@ if __name__ == "__main__":
     # # robot.arm.move_joint(q = robot.arm.off_pos)
     # # robot.arm.move_joint(q = robot.arm.init_pos)
     # pose1 = [0.353467,-0.1349880,0.403604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180]
-    # # robot.set_position(pose = pose1)
-    # print("arm_position1:",robot.arm.get_arm_position())
-    # print("end_position1:",robot.arm.get_end_position())
+    # pose1 = [0.3736677762541832, 0.11722785977006844, 0.3835904171566223, -3.141592653589793, -0.5235987755982988, 7.330382858376184e-05]
+    # pose1 = [0.4486,0.1172,0.2536,np.pi,-0.5236,0]
+    pose1 = [0.44866777605267877, 0.11723335755720732, 0.2536866065889565, 2.7069577869199146, -0.7176783797852828, 0.0]
+
+    robot.set_position(pose = pose1)
+    # print("get_pos:",robot.ge)
+    print("arm_position1:",robot.arm.get_arm_position())
+    print("end_position1:",robot.arm.get_end_position())
     # print("joint1:",robot.arm.getAngel())
     # robot.arm.move_joint(q=robot.arm.init_pos)
     # # robot.set_position([0.12189544299531646, -0.31364670468715944, 0.00733569473686102, 2.153932270623249, 0.6209756952505509, 0.0])

Massage/MassageControl/algorithms/hybridAdmit_controller.py

@@ -59,6 +59,8 @@ class HybridAdmitController(BaseController):
         wrench_err = self.state.external_wrench_tcp - self.state.desired_wrench
         # z方向力导纳控制
         self.state.arm_desired_acc[2] = 1/self.M_z * (wrench_err[2] - self.D_z * (self.state.arm_desired_twist[2] - self.state.desired_twist[2]) - self.K_z * self.state.pose_error[2])
+        # print("self.state.arm_desired_acc[2]:",self.state.arm_desired_acc[2],self.M_z,wrench_err[2],self.K_z,self.D_z)
+
         self.pose_integral_error +=  self.state.pose_error * dt
         # 位控制
         self.state.arm_desired_acc[:2] = -self.Kd @ self.state.arm_desired_twist[:2] - self.Kp @ self.state.pose_error[:2] - self.Ki @ self.pose_integral_error[:2]

Massage/MassageControl/algorithms/interpolation.py

@@ -359,7 +359,7 @@ def resample_curve_strict(points, num_resampled_points):
 
 
 
-def circle_trajectory(center, omega=8.0, radius=0.04, reverse=False, time_points=None, time_step=0.01, start_transition_duration=None, end_transition_duration=None):
+def circle_trajectory(center, omega=8.0, radius=0.06, reverse=False, time_points=None, time_step=0.01, start_transition_duration=None, end_transition_duration=None):
 
 
     if time_points is None or len(time_points) < 2:

Massage/MassageControl/config/hybrid.yaml

@@ -5,9 +5,9 @@ stiff_rot: [11, 11, 11]
 damp_rot: [1.1, 1.1, 1.1]
 desired_xi: 1.0
 
-mass_z: [3.0]
+mass_z: [7.0]
 stiff_z: [0]
-damp_z: [30]
+damp_z: [75]
 
 # 位控参数
 Kp: [20,20]

Massage/MassageControl/config/hybridAdmit.yaml

@@ -1,8 +1,8 @@
 name: 'hybridAdmit'
 
-mass_z: [3.0]
+mass_z: [7.0]
 stiff_z: [0]
-damp_z: [30]
+damp_z: [75]
 desired_xi: 1.0
 
 # 位控参数

Massage/MassageControl/hardware/dobot_api.py

@@ -6,6 +6,7 @@ import json
 import threading
 import time
 from time import sleep
+import time
 import select
 
 alarmControllerFile = "files/alarmController.json"
@@ -153,7 +154,7 @@ class DobotApi:
                     self.socket_dobot.send(str.encode(string, 'utf-8'))
                     break
                 except Exception:
-                    sleep(1)
+                    sleep(0.1)
 
     def wait_reply(self):
         """
@@ -161,7 +162,10 @@ class DobotApi:
         """
         data = ""
         try:
+            # print("time1:",time.time())
             data = self.socket_dobot.recv(1024)
+            # print("end_time:",time.time())
+            # print('recvdata:',data)
         except Exception as e:
             print(e)
             self.socket_dobot = self.reConnect(self.ip, self.port)
@@ -225,7 +229,10 @@ class DobotApi:
         """
         with self.__globalLock:
             self.send_data(string)
+            send_time = time.time()
+            print(f"send_data:{string}")
             recvData = self.wait_reply()
+            print(f"===={time.time()-send_time}s====recvData:{string}")
             if not self.ParseResultId(recvData):
                 return None
             return recvData

Massage/MassageControl/hardware/dobot_nova5.py

@@ -307,6 +307,8 @@ class dobot_nova5:
     
     def waitArrival(self, command):
         sendCommandID = self.parseResultprocess(command)[1]
+        # print("$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$")
+        self.logger.log_blue(f"self.feedbackData.robotMode")
         # print("self.feedbackData.robotMode6:",self.feedbackData.robotMode)
         # print("self.parseResultprocess(command)",self.parseResultprocess(command))
         while True:
@@ -317,7 +319,7 @@ class dobot_nova5:
                 self.dashboard.EmergencyStop(mode=1)
                 return -1 
             if self.feedbackData.EnableState:
-                # print("self.feedbackData.robotCurrentCommandID",self.feedbackData.robotCurrentCommandID)
+                # self.logger.log_blue("self.feedbackData.robotCurrentCommandID",self.feedbackData.robotCurrentCommandID)
                 if self.feedbackData.robotCurrentCommandID > sendCommandID:
                     break
                 else:
@@ -341,7 +343,7 @@ class dobot_nova5:
             command = self.dashboard.MovJ(*joints_list_deg,1)
             if command == None:
                 return -1
-
+            print("###################################")
             is_arrived = self.waitArrival(command)
             if is_arrived == 0:
                 # self.dashboard.MovJ()
@@ -413,10 +415,13 @@ class dobot_nova5:
         cnt = 0
         print("self.robotmode3:",self.feedbackData.robotMode)
         poses_list_mm_deg = self.__transform_m_rad_2_mm_deg(pose)
+        # self.feedFour.flush_recv_buffer()
+        # joint_list = self.getInverseKin(poses_list_mm_deg)
         while cnt < max_retry_count:
             cnt += 1
             print("self.robotmode4:",self.feedbackData.robotMode)
             command = self.dashboard.MovJ(*poses_list_mm_deg,0)
+            # command = self.dashboard.MovJ(*joint_list,1)
             if command == None:
                 return -1
             print("self.robotmode5:",self.feedbackData.robotMode)
@@ -648,7 +653,7 @@ class dobot_nova5:
         pose = [float(x.strip()) for x in pose_str.split(",")]
         return pose
     
-    def getInverseKin(self,poses_list,user=1,tool=-1,useJointNear=1,JointNear=''):
+    def getInverseKin(self,poses_list,user=1,tool=-1,useJointNear=0,JointNear=''):
         '''
             poses_list    X Y Z RX RY RZ
             useJointNear 0 或不携带 表示 JointNear无效
@@ -913,11 +918,9 @@ class dobot_nova5:
         return -1, None
     
     def chooseRightFrame(self):
-        self.chooseBaseFrame(i=1)
-        # self.chooseEndEffector(i=0)
+        self.setBaseFrame(i=1, base_i="{0,0,0,0,0,-45}")
         return
 
-
 if __name__ == "__main__":
 
     # print(R.from_euler("xyz", np.array([180, 0, 0]), degrees=True).as_matrix())
@@ -925,7 +928,7 @@ if __name__ == "__main__":
     
     dobot = dobot_nova5("192.168.5.1")
     dobot.init()
-    dobot.dashboard.LogExportUSB(0)
+    # dobot.dashboard.LogExportUSB(0)
 
     # # dobot.move_joint_jog(q=dobot.init_pos)
 
@@ -946,25 +949,25 @@ if __name__ == "__main__":
 
     # joint1 = [-44.8416*np.pi/180, 39.5626*np.pi/180, -80.5683*np.pi/180, -18.0396*np.pi/180, -89.4168*np.pi/180, -90.0026*np.pi/180]
     # try:
-    dobot.start_drag()
+    # dobot.start_drag()
     #     # dobot.move_joint(q = joint)
     #     # dobot.move_joint(q = joint1)
 
-    #     # pose1 = [0.353467,-0.1349880,0.403604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180]
+    # pose1 = [0.353467,-0.1349880,0.403604,-180.0000*np.pi/180,-30.0000*np.pi/180,0.0042*np.pi/180]
 
-    #     # dobot.RunPoint_P_inPose_M_RAD(pose=pose1)
+    # dobot.RunPoint_P_inPose_M_RAD(pose=pose1)
     try:
         while True:
             time.sleep(1)
     except KeyboardInterrupt:
         print("joint:",dobot.getAngel())
         # robot_thread.join()
-        dobot.stop_drag()
+        # dobot.stop_drag()
         print("用户中断操作。")
     except Exception as e:
         print("joint:",dobot.getAngel())
         # robot_thread.join()
-        dobot.stop_drag()
+        # dobot.stop_drag()
         print("Exception occurred at line:", e.__traceback__.tb_lineno)
         print("发生异常：", e)
     # # dobot.RunPoint_P_inJoint(posJ)

Massage/MassageControl/hardware/test.py

@@ -1,40 +1,24 @@
 import numpy as np
 import matplotlib.pyplot as plt
 from mpl_toolkits.mplot3d import Axes3D
+import copy
 from scipy.spatial.transform import Rotation as R
+import math
 
-# 定义你的坐标系（原点 + 欧拉角，单位为角度）
-frames = [
-    {"origin": [0, 0, 0], "euler": [0, 0, 0]},
-    {"origin": [2, 2, 2], "euler": [0, 150, 90]},
-    {"origin": [3, 3, 3], "euler": [-180, -30, 0]},
-]
 
-def draw_frame(ax, origin, rot_matrix, length=0.5):
-    origin = np.array(origin)
-    x_axis = origin + rot_matrix @ np.array([length, 0, 0])
-    y_axis = origin + rot_matrix @ np.array([0, length, 0])
-    z_axis = origin + rot_matrix @ np.array([0, 0, length])
-    ax.quiver(*origin, *(x_axis - origin), color='r')
-    ax.quiver(*origin, *(y_axis - origin), color='g')
-    ax.quiver(*origin, *(z_axis - origin), color='b')
+print(math.sin(math.atan2(0.4,0.84))*0.93)
 
-fig = plt.figure()
-ax = fig.add_subplot(111, projection='3d')
-ax.set_box_aspect([1,1,1])
+# point = np.array([0.3670603416735574, -0.36714597755113015, -0.8546756909720299])
 
-for frame in frames:
-    origin = frame["origin"]
-    euler_deg = frame["euler"]
-    rotation = R.from_euler('xyz', euler_deg, degrees=True)
-    rot_matrix = rotation.as_matrix()
-    draw_frame(ax, origin, rot_matrix)
 
-ax.set_xlim(-1, 3)
-ax.set_ylim(-1, 3)
-ax.set_zlim(-1, 3)
-ax.set_xlabel("X")
-ax.set_ylabel("Y")
-ax.set_zlabel("Z")
-plt.title("Multiple 3D Coordinate Frames (Euler Angles)")
-plt.show()
+# temp_point = copy.deepcopy(point)
+# if point[2]<= 0:
+#     temp_point[0] = -math.atan2(point[1],-np.sqrt(1-point[1]*point[1]))
+# else:
+#     temp_point[0] = -math.atan2(point[1],np.sqrt(1-point[1]*point[1])) 
+# temp_point[1] = -math.atan2(point[0], -point[2])
+# temp_point[2] = 0
+# # temp_point[0] = -math.asin(point[1])
+# # temp_point[1] = -math.atan2(-point[0],point[2])
+# # temp_point[2] = 0
+# print(temp_point)

Massage/MassageControl/tools/identifier_dobot.py

@@ -189,8 +189,8 @@ if __name__ == '__main__':
     sensor.disable_active_transmission()
     sensor.disable_active_transmission()
     atexit.register(sensor.disconnect)
-    identifier = Identifier(arm=arm,sensor=sensor,config_path="/home/jsfb/jsfb_ws/global_config/massage_head/finger_playload.yaml")
-    ready_pose = [0.353467*1000,-0.1349880*1000,0.403604*1000,-180.0000,-30.0000,0.0042]
+    identifier = Identifier(arm=arm,sensor=sensor,config_path="/home/jsfb/jsfb_ws/global_config/massage_head/shockwave_playload.yaml")
+    ready_pose = [0.403467*1000,-0.1349880*1000,0.423604*1000,-180.0000,-30.0000,0.0042]
     # ready_pose = [250.0, -135.0, 344.3392, -180.0, 0.0, -90.0]
     time.sleep(1)
     identifier.identify_param_auto(ready_pose,15)