add off pos
This commit is contained in:
LiangShiyun
parent
2183df1b0f
commit
5dcb63a51e
@ -578,7 +578,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_traj)
|
||||
self.arm_control_thread = threading.Thread(target=self.arm_command_loop)
|
||||
# 线程启动
|
||||
self.arm_measure_thread.start() ## 测量线程
|
||||
position,quat_rot = self.arm.get_arm_position()
|
||||
@ -629,8 +629,8 @@ class MassageRobot:
|
||||
|
||||
|
||||
self.controller_manager.switch_controller('position')
|
||||
else:
|
||||
self.logger.log_error(f"未找到{name}按摩头")
|
||||
# else:
|
||||
# self.logger.log_error(f"未找到{name}按摩头")
|
||||
|
||||
def log_thread(self):
|
||||
while True:
|
||||
@ -1617,35 +1617,35 @@ class MassageRobot:
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
waypoints = [
|
||||
{"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0],
|
||||
"acceleration": [0, 0, 0],
|
||||
"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],
|
||||
"acceleration": [0, 0, 0],
|
||||
"orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()},
|
||||
] ## 单位 m deg
|
||||
myInterpolate = TrajectoryInterpolator(waypoints)
|
||||
ts = np.linspace(0,5,100)
|
||||
robot = MassageRobot(arm_config_path="/home/jsfb/jsfb/MassageRobot_Dobot/Massage/MassageControl/config/robot_config.yaml")
|
||||
xr_list, vr_list, ar_list = [], [], []
|
||||
for t in ts:
|
||||
xr, vr, ar = myInterpolate.interpolate(t)
|
||||
xr_list.append(xr)
|
||||
vr_list.append(vr)
|
||||
ar_list.append(ar)
|
||||
xr_array = np.array(xr_list)
|
||||
vr_array = np.array(vr_list)
|
||||
ar_array = np.array(ar_list)
|
||||
# waypoints = [
|
||||
# {"time": 0.0, "position": [0.25, -0.135, 0.3443], "velocity": [0, 0, 0],
|
||||
# "acceleration": [0, 0, 0],
|
||||
# "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],
|
||||
# "acceleration": [0, 0, 0],
|
||||
# "orientation": R.from_euler("xyz", [0, 30, 30], degrees=True).as_quat()},
|
||||
# ] ## 单位 m deg
|
||||
# myInterpolate = TrajectoryInterpolator(waypoints)
|
||||
# ts = np.linspace(0,5,100)
|
||||
robot = MassageRobot(arm_config_path="/home/jsfb/jsfb_ws/MassageRobot_Dobot/Massage/MassageControl/config/robot_config.yaml")
|
||||
# xr_list, vr_list, ar_list = [], [], []
|
||||
# for t in ts:
|
||||
# xr, vr, ar = myInterpolate.interpolate(t)
|
||||
# xr_list.append(xr)
|
||||
# vr_list.append(vr)
|
||||
# ar_list.append(ar)
|
||||
# xr_array = np.array(xr_list)
|
||||
# vr_array = np.array(vr_list)
|
||||
# ar_array = np.array(ar_list)
|
||||
|
||||
robot.xr = xr_array
|
||||
robot.vr = vr_array
|
||||
robot.ar = ar_array
|
||||
robot.ts = ts
|
||||
robot.dt = ts[1] - ts[0]
|
||||
# robot.xr = xr_array
|
||||
# robot.vr = vr_array
|
||||
# robot.ar = ar_array
|
||||
# robot.ts = ts
|
||||
# robot.dt = ts[1] - ts[0]
|
||||
|
||||
|
||||
ready_pose = [250.0, -135.0, 344.3392, -180.0, 0.0, -90.0]
|
||||
|
||||
ready_pose = [204.3467,-134.9880,455.3604,-180.0000,-30.0000,0.0042]
|
||||
robot.current_head = 'finger_head'
|
||||
|
||||
robot.force_sensor.disable_active_transmission()
|
||||
@ -1666,15 +1666,18 @@ if __name__ == "__main__":
|
||||
|
||||
|
||||
atexit.register(robot.force_sensor.disconnect)
|
||||
robot.arm_state.desired_wrench = np.array([0,0,-0.5,0,0,0])
|
||||
robot.arm_state.desired_wrench = np.array([0,0,-1,0,0,0])
|
||||
try:
|
||||
robot_thread = threading.Thread(target=robot.start)
|
||||
robot_thread.start()
|
||||
except KeyboardInterrupt:
|
||||
robot_thread.join()
|
||||
print("用户中断操作。")
|
||||
except Exception as e:
|
||||
robot_thread.join()
|
||||
print("Exception occurred at line:", e.__traceback__.tb_lineno)
|
||||
print("发生异常：", e)
|
||||
|
||||
|
||||
|
||||
# robot.arm.disableRobot()
|
||||
@ -3,6 +3,7 @@ name: 'hybridAdmit'
|
||||
mass_z: [3.0]
|
||||
stiff_z: [0]
|
||||
damp_z: [30]
|
||||
desired_xi: 1.0
|
||||
|
||||
# 位控参数
|
||||
Kp: [20,20]
|
||||
|
||||
@ -2,11 +2,17 @@
|
||||
arm_ip: '192.168.5.1'
|
||||
|
||||
# controller
|
||||
controller: ['Massage/MassageControl/config/admittance.yaml',
|
||||
'Massage/MassageControl/config/position.yaml']
|
||||
controller: ['MassageControl/config/admittance.yaml',
|
||||
'MassageControl/config/hybrid.yaml',
|
||||
'MassageControl/config/position.yaml',
|
||||
'MassageControl/config/admithybrid.yaml',
|
||||
'MassageControl/config/hybridPid.yaml',
|
||||
'MassageControl/config/hybridAdmit.yaml',
|
||||
'MassageControl/config/positionerSensor.yaml',
|
||||
'MassageControl/config/admittanceZ.yaml',]
|
||||
|
||||
# massage heads diretory
|
||||
massage_head_dir: 'Massage/MassageControl/config/massage_head'
|
||||
massage_head_dir: '/home/jsfb/jsfb_ws/global_config/massage_head'
|
||||
|
||||
control_rate: 55
|
||||
sensor_rate: 55
|
||||
|
||||
@ -127,6 +127,7 @@ class DobotApi:
|
||||
if self.port == 29999 or self.port == 30004 or self.port == 30005:
|
||||
try:
|
||||
self.socket_dobot = socket.socket()
|
||||
self.socket_dobot.settimeout(5) # set time out 5s
|
||||
self.socket_dobot.connect((self.ip, self.port))
|
||||
self.socket_dobot.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 144000)
|
||||
except socket.error:
|
||||
@ -174,14 +175,32 @@ class DobotApi:
|
||||
|
||||
def close(self):
|
||||
"""
|
||||
Close the port
|
||||
Close the socket connection safely
|
||||
"""
|
||||
if (self.socket_dobot != 0):
|
||||
if isinstance(self.socket_dobot, socket.socket):
|
||||
try:
|
||||
self.socket_dobot.shutdown(socket.SHUT_RDWR)
|
||||
self.socket_dobot.close()
|
||||
except socket.error as e:
|
||||
print(f"Error while closing socket: {e}")
|
||||
print(f"[WARN] Error during socket shutdown: {e}")
|
||||
try:
|
||||
self.socket_dobot.close()
|
||||
print("[INFO] Socket closed successfully")
|
||||
except socket.error as e:
|
||||
print(f"[ERROR] Error while closing socket: {e}")
|
||||
finally:
|
||||
self.socket_dobot = None # 清理引用
|
||||
|
||||
|
||||
# def close(self):
|
||||
# """
|
||||
# Close the port
|
||||
# """
|
||||
# if (self.socket_dobot != 0):
|
||||
# try:
|
||||
# self.socket_dobot.shutdown(socket.SHUT_RDWR)
|
||||
# self.socket_dobot.close()
|
||||
# except socket.error as e:
|
||||
# print(f"Error while closing socket: {e}")
|
||||
|
||||
def sendRecvMsg(self, string):
|
||||
"""
|
||||
@ -3050,7 +3069,13 @@ class DobotApiDashboard(DobotApi):
|
||||
|
||||
class DobotApiFeedBack(DobotApi):
|
||||
def __init__(self, ip, port, *args):
|
||||
super().__init__(ip, port, *args)
|
||||
try:
|
||||
super().__init__(ip, port, *args)
|
||||
except Exception as e:
|
||||
print(f"[错误] 无法连接 Dobot（IP: {ip}, Port: {port}）：{e}")
|
||||
# 可根据需要抛出异常或安全退出
|
||||
raise # 或者：self.connected = False; return
|
||||
# super().__init__(ip, port, *args)
|
||||
self.__MyType = []
|
||||
self.last_recv_time = time.perf_counter()
|
||||
|
||||
|
||||
@ -149,6 +149,7 @@ class dobot_nova5:
|
||||
|
||||
atexit.register(self.exit_task)
|
||||
self.init_pos = [-45.0009079*np.pi/180,55.5785141*np.pi/180,-120.68821716*np.pi/180,5.11103201*np.pi/180,90.00195312*np.pi/180,-90.00085449*np.pi/180]
|
||||
self.off_pos = [-44.996*np.pi/180,87.0092*np.pi/180,-147.0087*np.pi/180,-0.0048*np.pi/180,90.0010*np.pi/180,-90.0021*np.pi/180]
|
||||
|
||||
# 为状态管理而封装的初始化函数
|
||||
def init(self):
|
||||
@ -207,7 +208,7 @@ class dobot_nova5:
|
||||
"http://127.0.0.1:5000/update_massage_status", data=instruction
|
||||
)
|
||||
time.sleep(1)
|
||||
# self.move_joint(self.off_pos)
|
||||
self.move_joint(self.off_pos)
|
||||
self.stop_feedback.set()
|
||||
self.feedback_thread.join()
|
||||
self.dashboard.Disconnect()
|
||||
|
||||
@ -15,7 +15,7 @@ sys.path.append(os.path.abspath(os.path.join(os.getcwd(), "Massage/MassageContro
|
||||
|
||||
|
||||
class XjcSensor:
|
||||
def __init__(self, port=None, baudrate=115200, rate=250):
|
||||
def __init__(self, port='/dev/ttyS0', baudrate=115200, rate=250):
|
||||
self.port = port
|
||||
self.baudrate = baudrate
|
||||
self.rate = rate
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user