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:19:18 +08:00
parent 04364f7a95
commit 52c84bb914
1 changed files with 1647 additions and 1628 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
Massage/MassageControl/MassageRobot_nova5.py
View File

@ -113,9 +113,14 @@ from functools import wraps
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()
<<<<<<< HEAD
self.arm.chooseRightFrame()
=======
self.arm.init() self.arm.init()
>>>>>>> cc63eb47a5fd86a36d8af9fbf38a4b10a4c2ff7f
self.thermotherapy = None self.thermotherapy = None
self.shockwave = None self.shockwave = None
@ -130,18 +135,30 @@ from functools import wraps
# 控制器,初始为导纳控制 # 控制器,初始为导纳控制
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'][2]) self.controller_manager.add_controller(PositionController,'position',self.arm_config['controller'][1])
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')
<<<<<<< HEAD
# 读取数据
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()
=======
>>>>>>> cc63eb47a5fd86a36d8af9fbf38a4b10a4c2ff7f
# 频率数据赋值 # 频率数据赋值
self.control_rate = Rate(self.arm_config['control_rate']) self.control_rate = Rate(self.arm_config['control_rate'])
@ -540,8 +557,9 @@ from functools import wraps
command_pose[:3], command_pose[:3],
command_pose[3:] command_pose[3:]
) )
# print(f"pose_processed:{pose_processed}") # pose_processed = command_pose
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},"
@ -1568,35 +1586,35 @@ from functools import wraps
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 = [250.0, -135.0, 344.3392, -180.0, 0.0, -90.0] ready_pose = [194.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042]
robot.current_head = 'finger_head' robot.current_head = 'finger_head'
robot.force_sensor.disable_active_transmission() robot.force_sensor.disable_active_transmission()
@ -1628,3 +1646,4 @@ from functools import wraps
print("发生异常：", e) print("发生异常：", e)
# robot.arm.disableRobot()