修改massagrobot_nova5
This commit is contained in:
swayneleong
parent
902a36ef4d
commit
cc63eb47a5
@ -108,11 +108,14 @@ class MassageRobot:
|
||||
|
||||
self.vtxdb = VTXClient()
|
||||
|
||||
# 当前执行的函数
|
||||
self.current_function = None
|
||||
self.arm_state = ArmState()
|
||||
self.arm_config = read_yaml(arm_config_path)
|
||||
# arm 实例化时机械臂类内部进行通讯连接
|
||||
self.arm = dobot_nova5(arm_ip = self.arm_config['arm_ip'])
|
||||
self.arm.start()
|
||||
self.arm.init()
|
||||
|
||||
self.thermotherapy = None
|
||||
self.shockwave = None
|
||||
@ -136,26 +139,9 @@ class MassageRobot:
|
||||
self.controller_manager.add_controller(AdmittanceZController,'admittanceZ',self.arm_config['controller'][7])
|
||||
|
||||
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,0,-90])
|
||||
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'])
|
||||
@ -177,15 +163,30 @@ class MassageRobot:
|
||||
# 按摩调整
|
||||
self.adjust_wrench_envent = threading.Event()
|
||||
self.adjust_wrench_envent.clear() # 调整初始化为False
|
||||
self.is_execute = False
|
||||
self.pos_increment = np.zeros(3,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.last_print_time = 0
|
||||
self.last_record_time = 0
|
||||
self.last_command_time = 0
|
||||
self.move_to_point_count = 0
|
||||
self.width_default = 5
|
||||
# 卡尔曼滤波相关初始化
|
||||
self.x_base = np.zeros(6)
|
||||
self.P_base = np.eye(6)
|
||||
@ -203,7 +204,16 @@ class MassageRobot:
|
||||
# 传感器故障计数器
|
||||
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
|
||||
self.xr = []
|
||||
@ -215,6 +225,36 @@ class MassageRobot:
|
||||
self.last_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):
|
||||
self.arm_position_set0,self.arm_orientation_set0 = self.arm.get_arm_position()
|
||||
@ -323,30 +363,7 @@ class MassageRobot:
|
||||
return -1
|
||||
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):
|
||||
# 当前的机械臂到末端转换 (实时)
|
||||
@ -383,6 +400,29 @@ class MassageRobot:
|
||||
self.arm_state.external_wrench_tcp = self.x_tcp
|
||||
self.arm_state.last_external_wrench_tcp = self.arm_state.external_wrench_tcp
|
||||
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}按摩头")
|
||||
|
||||
# ####################### 位姿伺服 ##########################
|
||||
|
||||
@ -535,7 +575,7 @@ class MassageRobot:
|
||||
# 线程
|
||||
self.exit_event.clear()
|
||||
self.arm_measure_thread = threading.Thread(target=self.arm_measure_loop)
|
||||
self.arm_control_thread = threading.Thread(target=self.arm_command_loop)
|
||||
self.arm_control_thread = threading.Thread(target=self.arm_command_loop_traj)
|
||||
# 线程启动
|
||||
self.arm_measure_thread.start() ## 测量线程
|
||||
position,quat_rot = self.arm.get_arm_position()
|
||||
@ -584,34 +624,7 @@ class MassageRobot:
|
||||
self.arm.stop_motion()
|
||||
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')
|
||||
else:
|
||||
self.logger.log_error(f"未找到{name}按摩头")
|
||||
@ -1608,12 +1621,11 @@ if __name__ == "__main__":
|
||||
try:
|
||||
robot_thread = threading.Thread(target=robot.start)
|
||||
robot_thread.start()
|
||||
|
||||
except KeyboardInterrupt:
|
||||
print("用户中断操作。")
|
||||
except Exception as e:
|
||||
print("Exception occurred at line:", e.__traceback__.tb_lineno)
|
||||
print("发生异常：", e)
|
||||
|
||||
robot_thread.join() # 等待机械臂线程结束
|
||||
|
||||
|
||||
# robot.arm.disableRobot()
|
||||
Loading…
x
Reference in New Issue
Block a user