import rospy
from sensor_msgs.msg import Joy
import time
import pygame
from ElfinRobotMovementHTIC import *
class Cmd(object):
def __init__(self):
self.reset()
def reset(self):
self.axis0 = 0
self.axis1 = 0
self.axis2 = 0
self.axis3 = 0
self.axis4 = 0
self.axis5 = 0
self.btn0 = 0
self.btn1 = 0
self.btn2 = 0
self.btn3 = 0
self.btn4 = 0
self.btn5 = 0
self.btn6 = 0
self.btn7 = 0
self.btn8 = 0
self.btn9 = 0
self.hat0 = 0
class Service(object):
def __init__(self, robot):
self.joystick = None
self.robot = robot
self.cmd = Cmd()
def init_joystick(self):
pygame.init()
self.joystick = pygame.joystick.Joystick(0)
self.joystick.init()
print("44444444")
def test(self,wet):
print(wet)
def loop(self):
air = False
while True:
self.cmd.reset()
pygame.event.pump()
for i in range(0, self.joystick.get_numaxes()):
val = self.joystick.get_axis(i)
if abs(val) < 0.2:
val = 0
tmp = "self.cmd.axis" + str(i) + " = " + str(val)
if val != 0:
exec(tmp)
for i in range(0, self.joystick.get_numhats()):
val = self.joystick.get_hat(i)
tmp = "self.cmd.hat" + str(i) + " = " + str(val)
exec(tmp)
for i in range(0, self.joystick.get_numbuttons()):
if self.joystick.get_button(i) != 0:
tmp = "self.cmd.btn" + str(i) + " = 1"
exec(tmp)
if self.cmd.btn1:
#toggle IO
air = not air
stoprobot(self.robot)
#(0:X,1:Y,2:Z,3:A,4:B,5:C)
#0=negative;1=positive
if self.cmd.btn0 and self.cmd.hat0 == (0, 1):
print("value from the hat is,self.cmd.btn0",self.cmd.hat0,self.cmd.btn0)
LongJogL(self.robot,2,1)
elif self.cmd.btn0 and self.cmd.hat0 == (0, -1):
print("value from the hat is,self.cmd.hat0",self.cmd.hat0,self.cmd.btn0)
LongJogL(self.robot,2,0)
elif self.cmd.hat0 == (0, 1):
print("value from the hat is",self.cmd.hat0)
self.test("test")
LongJogL(self.robot,1,0)
elif self.cmd.hat0 == (0, -1):
print("value from the hat is",self.cmd.hat0)
LongJogL(self.robot,1,1)
elif self.cmd.hat0 == (1, 0):
print("value from the hat is",self.cmd.hat0)
LongJogL(self.robot,0,0)
elif self.cmd.hat0 == (-1, 0):
print("value from the hat is",self.cmd.hat0)
LongJogL(self.robot,0,1)
elif self.cmd.axis0 > 0.5 :
LongJogL(self.robot,3,1)
print("movement in axis0")
elif self.cmd.axis0 < -0.5:
LongJogL(self.robot,3,0)
print("movement in axis0")
elif self.cmd.axis1 > 0.5:
LongJogL(self.robot,4,1)
elif self.cmd.axis1 < -0.5:
LongJogL(self.robot,4,0)
elif self.cmd.axis2 > 0.5:
LongJogL(self.robot,5,1)
elif self.cmd.axis2 < -0.5:
LongJogL(self.robot,5,0)
else:
stoprobot(self.robot)
def close(self):
if self.joystick:
self.joystick.quit()
if __name__ == "__main__":
s = connect('192.168.2.1')
s.send(b"ReadRobotState,0,;\n")
response = s.recv(1024).decode() # 示例响应: "ReadRobotState,OK,1,"
print(f"机械臂状态: {response}")
service = Service(s)
service.init_joystick()
try:
service.loop()
finally:
service.close()
以及import math
from math import *
import math3d as m3d
import numpy as np
import time
import socket
import os
# import tqdm
# #-------------------General Modules
import socket
import numpy as np
import os
import math
# #-------------------Math and Robot Modules
from scipy.spatial.transform import Rotation as R
import matplotlib.pyplot as plt
# #-------------------Core Robotic Modules
ip = "192.168.2.1"
port = int("10003")
#---------------------------------------------------------------------------------------------------------
#Modules to Make Motion of the Robot with respect to the Robot
def connect(ip = "192.168.2.1", port = 10003):
"""Connect to the robot
Args:
ip ([String]): [192.168.2.1]
port ([Int]): [10003]
Returns:
[Object]: [soccet communication s]
"""
try:
s = socket.socket()
s.settimeout(5)
s.connect((ip, int(port)))
print('robot connected succesfully to')
print('ip - {}'.format(ip), '\n', 'port - {}'.format(port))
return s
except Exception as e:
print(e)
print("cant retrive the Actual Position of the Robot")
def LongJogL(s, axis, direction, port=10003, robotID=0):
try:
data = f"LongJogL,{robotID},{axis},{direction},;\n" # 构建指令
print(f"发送指令: {data.strip()}") # 打印发送的指令
s.send(data.encode())
result = s.recv(1024).decode() # 接收响应（增大缓冲区，避免截断）
print(f"机械臂响应: {result.strip()}") # 打印响应
# ... 其他逻辑 ...
except Exception as e:
print(f"指令发送失败: {e}")
# def LongJogL(s, axis,direction, port=10003, robotID=0):
# """[Move the robot using Joint Values]
# Args:
# s ([Object]): [Robot TCP Connection]
# angles ([List of Floats]): [Joint angles]
# port (int, optional): [Port you got connected to]. Defaults to 10003.
# robotID (int, optional): [The unique identifier of the robot]. Defaults to 0.
# """
# try:
# # x, y, z, rx, ry, rz = tuple(pose)
# data = str("LongJogL,{},{},{},;\n".format(robotID, axis,direction))
# # print(data)
# data = data.encode()
# s.send(data)
# result = s.recv(port).decode()
# # print('result is',result)
# # cut = result.split(',')
# # if(cut[1] == "OK"):
# # while 1:
# # s.send("ReadRobotState,0,;".encode())
# # result = s.recv(port).decode()
# # cut = result.split(',')
# # # print('cut',cut)
# # if(cut[1] == "OK" and cut[2] == "0"):
# # break
# # elif(cut[1] == "Fail"):
# # print(result)
# except Exception as e:
# print(e)
# print("cant retrive the Actual Position of the Robot")
# def LongJogL(s, axis, direction, port=10003, robotID=0):
# try:
# data = str("LongJogL,{},{},{},;\n".format(robotID, axis, direction))
# data = data.encode()
# s.send(data)
# result = s.recv(1024).decode() # 修改了port为固定1024
# print('result is', result)
# if "Fail" in result:
# print(f"运动指令失败: {result}")
# return False
# return True
# except Exception as e:
# print(f"通信错误: {e}")
# return False
def stoprobot(s, port=10003, robotID=0):
try:
data = str("GrpStop,0,;")
# print(data)
data = data.encode()
s.send(data)
result = s.recv(port).decode()
# print('result is',result)
cut = result.split(',')
if(cut[1] == "OK"):
while 1:
s.send("ReadRobotState,0,;".encode())
result = s.recv(port).decode()
cut = result.split(',')
# print('cut',cut)
if(cut[1] == "OK" and cut[2] == "0"):
break
elif(cut[1] == "Fail"):
print(result)
except Exception as e:
print(e)
print("cant stop robot")
def check_robot_status(s, robotID=0):
"""检查机械臂状态"""
try:
s.send(f"ReadRobotState,{robotID},;\n".encode())
response = s.recv(1024).decode()
if "OK,0" in response:
return True
print(f"机械臂状态异常: {response}")
return False
except Exception as e:
print(f"状态检查错误: {e}")
return False
def connect(ip="192.168.2.1", port=10003):
"""增强版连接函数"""
try:
s = socket.socket()
s.settimeout(3.0) # 设置超时
s.connect((ip, port))
# 验证连接
s.send(b"ReadRobotState,0,;\n")
if b"OK" in s.recv(1024):
print(f"成功连接到机械臂 {ip}:{port}")
return s
else:
s.close()
raise Exception("机械臂响应异常")
except Exception as e:
print(f"连接失败: {e}")
return None
；这两个代码实现手柄控制机械臂运动，并借用pygame，TCP通信，你可以完善代码，并且这是两个代码，你需要给我两个代码完善后的完整代码
扫一扫
1987
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
