DevelopmentTeam/MassageRobot_Dobot
15
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

暂存修改

Browse Source
This commit is contained in:
Ziwei.He 2025-05-28 21:18:29 +08:00
commit 04364f7a95
1 changed files with 1628 additions and 1620 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

230
Massage/MassageControl/MassageRobot_nova5.py
View File

@ -108,13 +108,14 @@ class MassageRobot:
self.vtxdb = VTXClient() self.vtxdb = VTXClient()
# 当前执行的函数
self.current_function = None
self.arm_state = ArmState() self.arm_state = ArmState()
self.arm_config = read_yaml(arm_config_path) self.arm_config = read_yaml(arm_config_path)
# arm 实例化时机械臂类内部进行通讯连接 # arm 实例化时机械臂类内部进行通讯连接
# 初始化坐标系 TOOL=0 BASE=1
self.arm = dobot_nova5(arm_ip = self.arm_config['arm_ip']) self.arm = dobot_nova5(arm_ip = self.arm_config['arm_ip'])
self.arm.start() self.arm.start()
self.arm.chooseRightFrame() self.arm.init()
self.thermotherapy = None self.thermotherapy = None
self.shockwave = None self.shockwave = None
@ -129,35 +130,18 @@ class MassageRobot:
# 控制器,初始为导纳控制 # 控制器,初始为导纳控制
self.controller_manager = ControllerManager(self.arm_state) self.controller_manager = ControllerManager(self.arm_state)
self.controller_manager.add_controller(AdmittanceController,'admittance',self.arm_config['controller'][0]) self.controller_manager.add_controller(AdmittanceController,'admittance',self.arm_config['controller'][0])
# self.controller_manager.add_controller(HybridController,'hybrid',self.arm_config['controller'][1]) self.controller_manager.add_controller(HybridController,'hybrid',self.arm_config['controller'][1])
self.controller_manager.add_controller(PositionController,'position',self.arm_config['controller'][1]) self.controller_manager.add_controller(PositionController,'position',self.arm_config['controller'][2])
# self.controller_manager.add_controller(AdmitHybridController,'admithybrid',self.arm_config['controller'][3]) self.controller_manager.add_controller(AdmitHybridController,'admithybrid',self.arm_config['controller'][3])
# self.controller_manager.add_controller(HybridPidController,'hybridPid',self.arm_config['controller'][4]) self.controller_manager.add_controller(HybridPidController,'hybridPid',self.arm_config['controller'][4])
# self.controller_manager.add_controller(HybridAdmitController,'hybridAdmit',self.arm_config['controller'][5]) self.controller_manager.add_controller(HybridAdmitController,'hybridAdmit',self.arm_config['controller'][5])
# self.controller_manager.add_controller(PositionerSensorController,'positionerSensor',self.arm_config['controller'][6]) self.controller_manager.add_controller(PositionerSensorController,'positionerSensor',self.arm_config['controller'][6])
# self.controller_manager.add_controller(AdmittanceZController,'admittanceZ',self.arm_config['controller'][7]) self.controller_manager.add_controller(AdmittanceZController,'admittanceZ',self.arm_config['controller'][7])
self.controller_manager.switch_controller('admittance') self.controller_manager.switch_controller('admittance')
# 按摩头参数暂时使用本地数据
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.current_head = 'none'
# 读取数据
self.gravity_base = None
self.force_zero = None
self.torque_zero = None
self.tool_position = None
self.mass_center_position = None
self.s_tf_matrix_t = None
arm_orientation_set0 = np.array([-180,-30,0])
self.b_rotation_s_set0 = R.from_euler('xyz',arm_orientation_set0,degrees=True).as_matrix()
# 频率数据赋值 # 频率数据赋值
self.control_rate = Rate(self.arm_config['control_rate']) self.control_rate = Rate(self.arm_config['control_rate'])
@ -179,15 +163,30 @@ class MassageRobot:
# 按摩调整 # 按摩调整
self.adjust_wrench_envent = threading.Event() self.adjust_wrench_envent = threading.Event()
self.adjust_wrench_envent.clear() # 调整初始化为False self.adjust_wrench_envent.clear() # 调整初始化为False
self.is_execute = False
self.pos_increment = np.zeros(3,dtype=np.float64) self.pos_increment = np.zeros(3,dtype=np.float64)
self.adjust_wrench = np.zeros(6,dtype=np.float64) 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
# self.playload_dict = self.vtxdb.get("robot_config", "massage_head")
# print(self.playload_dict)
self.current_head = 'none'
self.is_waitting = False self.is_waitting = False
self.last_print_time = 0 self.last_print_time = 0
self.last_record_time = 0 self.last_record_time = 0
self.last_command_time = 0 self.last_command_time = 0
self.move_to_point_count = 0 self.move_to_point_count = 0
self.width_default = 5
# 卡尔曼滤波相关初始化 # 卡尔曼滤波相关初始化
self.x_base = np.zeros(6) self.x_base = np.zeros(6)
self.P_base = np.eye(6) self.P_base = np.eye(6)
@ -205,7 +204,16 @@ class MassageRobot:
# 传感器故障计数器 # 传感器故障计数器
self.sensor_fail_count = 0 self.sensor_fail_count = 0
# 机械臂初始化,适配中间层 # 机械臂初始化,适配中间层
self.arm.init() # 读取数据
self.gravity_base = None
self.force_zero = None
self.torque_zero = None
self.tool_position = None
self.mass_center_position = None
self.s_tf_matrix_t = None
arm_orientation_set0 = np.array([-180,0,-90])
self.b_rotation_s_set0 = R.from_euler('xyz',arm_orientation_set0,degrees=True).as_matrix()
# REF TRAJ # REF TRAJ
self.xr = [] self.xr = []
@ -217,6 +225,36 @@ class MassageRobot:
self.last_time = -1 self.last_time = -1
self.cur_time = -1 self.cur_time = -1
# 预测步骤
def kalman_predict(self,x, P, Q):
# 预测状态（这里假设状态不变）
x_predict = x
# 预测误差协方差
P_predict = P + Q
return x_predict, P_predict
# 更新步骤
def kalman_update(self,x_predict, P_predict, z, R):
# 卡尔曼增益
# K = P_predict @ np.linalg.inv(P_predict + R)
S = P_predict + R
s = np.diag(S) # shape (6,)
p_diag = np.diag(P_predict)
K_diag = np.zeros_like(s)
nonzero_mask = s != 0
K_diag[nonzero_mask] = p_diag[nonzero_mask] / s[nonzero_mask]
K = np.diag(K_diag)
# 更新状态
x_update = x_predict + K @ (z - x_predict)
# 更新误差协方差
P_update = (np.eye(len(K)) - K) @ P_predict
return x_update, P_update
def init_hardwares(self,ready_pose):
self.ready_pose = np.array(ready_pose)
self.switch_payload(self.current_head)
print(self.arm.get_arm_position())
time.sleep(0.5)
def sensor_set_zero(self): def sensor_set_zero(self):
self.arm_position_set0,self.arm_orientation_set0 = self.arm.get_arm_position() self.arm_position_set0,self.arm_orientation_set0 = self.arm.get_arm_position()
@ -325,30 +363,7 @@ class MassageRobot:
return -1 return -1
return 0 return 0
# 预测步骤
def kalman_predict(self,x, P, Q):
# 预测状态（这里假设状态不变）
x_predict = x
# 预测误差协方差
P_predict = P + Q
return x_predict, P_predict
# 更新步骤
def kalman_update(self,x_predict, P_predict, z, R):
# 卡尔曼增益
# K = P_predict @ np.linalg.inv(P_predict + R)
S = P_predict + R
s = np.diag(S) # shape (6,)
p_diag = np.diag(P_predict)
K_diag = np.zeros_like(s)
nonzero_mask = s != 0
K_diag[nonzero_mask] = p_diag[nonzero_mask] / s[nonzero_mask]
K = np.diag(K_diag)
# 更新状态
x_update = x_predict + K @ (z - x_predict)
# 更新误差协方差
P_update = (np.eye(len(K)) - K) @ P_predict
return x_update, P_update
def update_wrench(self): def update_wrench(self):
# 当前的机械臂到末端转换 (实时) # 当前的机械臂到末端转换 (实时)
@ -386,6 +401,29 @@ class MassageRobot:
self.arm_state.last_external_wrench_tcp = self.arm_state.external_wrench_tcp self.arm_state.last_external_wrench_tcp = self.arm_state.external_wrench_tcp
return 0 return 0
def switch_payload(self,name):
if name in self.playload_dict:
self.stop()
self.current_head = name
compensation_config = self.playload_dict[self.current_head]
# 读取数据
self.gravity_base = np.array(compensation_config['gravity_base'])
self.force_zero = np.array(compensation_config['force_zero'])
self.torque_zero = np.array(compensation_config['torque_zero'])
self.tool_position = np.array(compensation_config['tcp_offset'])
self.mass_center_position = np.array(compensation_config['mass_center_position'])
self.s_tf_matrix_t = self.get_tf_matrix(self.tool_position[:3], R.from_euler('xyz',self.tool_position[3:],degrees=False).as_quat())
tcp_offset = self.playload_dict[name]["tcp_offset"]
tcp_offset_str = "{" + ",".join(map(str, tcp_offset)) + "}"
print("tcp_offset_str",tcp_offset_str)
self.arm.setEndEffector(i=9,tool_i=tcp_offset_str)
self.arm.chooseEndEffector(i=9)
print(self.arm.get_arm_position())
self.logger.log_info(f"切换到{name}按摩头")
# ####################### 位姿伺服 ########################## # ####################### 位姿伺服 ##########################
def arm_measure_loop(self): def arm_measure_loop(self):
@ -502,9 +540,8 @@ class MassageRobot:
command_pose[:3], command_pose[:3],
command_pose[3:] command_pose[3:]
) )
# pose_processed = command_pose # print(f"pose_processed:{pose_processed}")
print(f"pose_processed:{pose_processed}")
print(self.arm.feedbackData.robotMode)
tcp_command = ( tcp_command = (
f"ServoP({pose_processed[0]:.3f},{pose_processed[1]:.3f}," f"ServoP({pose_processed[0]:.3f},{pose_processed[1]:.3f},"
f"{pose_processed[2]:.3f},{pose_processed[3]:.3f}," f"{pose_processed[2]:.3f},{pose_processed[3]:.3f},"
@ -538,7 +575,7 @@ class MassageRobot:
# 线程 # 线程
self.exit_event.clear() self.exit_event.clear()
self.arm_measure_thread = threading.Thread(target=self.arm_measure_loop) self.arm_measure_thread = threading.Thread(target=self.arm_measure_loop)
self.arm_control_thread = threading.Thread(target=self.arm_command_loop) self.arm_control_thread = threading.Thread(target=self.arm_command_loop_traj)
# 线程启动 # 线程启动
self.arm_measure_thread.start() ## 测量线程 self.arm_measure_thread.start() ## 测量线程
position,quat_rot = self.arm.get_arm_position() position,quat_rot = self.arm.get_arm_position()
@ -587,34 +624,7 @@ class MassageRobot:
self.arm.stop_motion() self.arm.stop_motion()
self.logger.log_info("MassageRobot停止") self.logger.log_info("MassageRobot停止")
def init_hardwares(self,ready_pose):
self.ready_pose = np.array(ready_pose)
self.switch_payload(self.current_head)
print(self.arm.get_arm_position())
time.sleep(0.5)
def switch_payload(self,name):
if name in self.playload_dict:
self.stop()
self.current_head = name
compensation_config = self.playload_dict[self.current_head]
# 读取数据
self.gravity_base = np.array(compensation_config['gravity_base'])
self.force_zero = np.array(compensation_config['force_zero'])
self.torque_zero = np.array(compensation_config['torque_zero'])
self.tool_position = np.array(compensation_config['tcp_offset'])
self.mass_center_position = np.array(compensation_config['mass_center_position'])
self.s_tf_matrix_t = self.get_tf_matrix(self.tool_position[:3], R.from_euler('xyz',self.tool_position[3:],degrees=False).as_quat())
tcp_offset = self.playload_dict[name]["tcp_offset"]
tcp_offset_str = "{" + ",".join(map(str, tcp_offset)) + "}"
print("tcp_offset_str",tcp_offset_str)
self.arm.setEndEffector(i=9,tool_i=tcp_offset_str)
self.arm.chooseEndEffector(i=9)
print(self.arm.get_arm_position())
self.logger.log_info(f"切换到{name}按摩头")
self.controller_manager.switch_controller('position') self.controller_manager.switch_controller('position')
else: else:
self.logger.log_error(f"未找到{name}按摩头") self.logger.log_error(f"未找到{name}按摩头")
@ -1558,35 +1568,35 @@ class MassageRobot:
if __name__ == "__main__": if __name__ == "__main__":
# waypoints = [ waypoints = [
# {"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0], {"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0],
# "acceleration": [0, 0, 0], "acceleration": [0, 0, 0],
# "orientation": R.from_euler("xyz", [0, 0, 0], degrees=True).as_quat()}, "orientation": R.from_euler("xyz", [0, 0, 0], degrees=True).as_quat()},
# {"time": 5.0, "position": [0.30, -0.135, 0.3043], "velocity": [0, 0, 0], {"time": 5.0, "position": [0.30, -0.135, 0.3043], "velocity": [0, 0, 0],
# "acceleration": [0, 0, 0], "acceleration": [0, 0, 0],
# "orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()}, "orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()},
# ] ## 单位 m deg ] ## 单位 m deg
# myInterpolate = TrajectoryInterpolator(waypoints) myInterpolate = TrajectoryInterpolator(waypoints)
# ts = np.linspace(0,5,100) ts = np.linspace(0,5,100)
robot = MassageRobot(arm_config_path="/home/jsfb/jsfb/MassageRobot_Dobot/Massage/MassageControl/config/robot_config.yaml") robot = MassageRobot(arm_config_path="/home/jsfb/jsfb/MassageRobot_Dobot/Massage/MassageControl/config/robot_config.yaml")
# xr_list, vr_list, ar_list = [], [], [] xr_list, vr_list, ar_list = [], [], []
# for t in ts: for t in ts:
# xr, vr, ar = myInterpolate.interpolate(t) xr, vr, ar = myInterpolate.interpolate(t)
# xr_list.append(xr) xr_list.append(xr)
# vr_list.append(vr) vr_list.append(vr)
# ar_list.append(ar) ar_list.append(ar)
# xr_array = np.array(xr_list) xr_array = np.array(xr_list)
# vr_array = np.array(vr_list) vr_array = np.array(vr_list)
# ar_array = np.array(ar_list) ar_array = np.array(ar_list)
# robot.xr = xr_array robot.xr = xr_array
# robot.vr = vr_array robot.vr = vr_array
# robot.ar = ar_array robot.ar = ar_array
# robot.ts = ts robot.ts = ts
# robot.dt = ts[1] - ts[0] robot.dt = ts[1] - ts[0]
ready_pose = [194.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042] ready_pose = [250.0, -135.0, 344.3392, -180.0, 0.0, -90.0]
robot.current_head = 'finger_head' robot.current_head = 'finger_head'
robot.force_sensor.disable_active_transmission() robot.force_sensor.disable_active_transmission()
@ -1611,12 +1621,10 @@ if __name__ == "__main__":
try: try:
robot_thread = threading.Thread(target=robot.start) robot_thread = threading.Thread(target=robot.start)
robot_thread.start() robot_thread.start()
except KeyboardInterrupt: except KeyboardInterrupt:
print("用户中断操作。") print("用户中断操作。")
except Exception as e: except Exception as e:
print("Exception occurred at line:", e.__traceback__.tb_lineno) print("Exception occurred at line:", e.__traceback__.tb_lineno)
print("发生异常：", e) print("发生异常：", e)
robot_thread.join() # 等待机械臂线程结束
# robot.arm.disableRobot()