若希望机器人绕当前TCP末端旋转啊一定角度,包括参考TCP方向或者base方向,可以使用以下代码计算并运动
import math import socket import json import time # Author:chenliao@elibot.cn # 绕当前末端TCP点旋转,可以参考tcp方向或者base方向 def Rotate(sock,p,toolorbase=0,rx=0,ry=0,rz=0): # 输入参考笛卡尔空间点,后续旋转rx,ry,rz为deg out = [0,0,0,math.radians(rx),math.radians(ry),math.radians(rz)] if toolorbase==0: # 绕着当前TCP的方向转 suc, result , id=sendCMD(sock,"poseMul",{"pose1":p,"pose2":out}) if toolorbase==1: # 绕着当前末端,参考base坐标系转 p_tmp = [0,0,0,p[3],p[4],p[5]] suc, result , id=sendCMD(sock,"poseMul",{"pose1":out,"pose2":p_tmp}) result[0] =p[0] result[1] =p[1] result[2] =p[2] return result def connectETController(ip, port=8055): sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) try: sock.connect((ip, port)) return (True, sock) except Exception as e: sock.close() return (False,) def disconnectETController(sock): if (sock): sock.close() sock = None else: sock = None def sendCMD(sock, cmd, params=None, id=1): if (not params): params = [] else: params = json.dumps(params) sendStr = "{{\"method\":\"{0}\",\"params\":{1},\"jsonrpc\":\"2.0\",\"id\":{2}}}".format(cmd, params, id) + "\n" try: # print(sendStr) sock.sendall(bytes(sendStr, "utf-8")) ret = sock.recv(1024) jdata = json.loads(str(ret, "utf-8")) if ("result" in jdata.keys()): return (True, json.loads(jdata["result"]), jdata["id"]) elif ("error" in jdata.keys()): print(sock.error["code"]) print(sock.error["message"]) return (False, json.loads(jdata["error"]), jdata["id"]) else: return (False, None, None) except Exception as e: return (False, None, None) # 等待机器人完成动作 def wait_stop(): while True: time.sleep(0.01) ret1, result1, id1 = sendCMD(sock, "getRobotState") # print(ret1,result1) if (ret1): if result1 == 0 or result1 == 4: break else: print("getRobotState failed") break def checkPose(sock,p,delta): # 判断机器人当前xyz坐标与传入的p的xyz坐标, # 如果3个方向的差值均小于delta,返回True ret1, c_pose, id = sendCMD(sock, "get_tcp_pose") if abs(c_pose[0]-p[0])<delta and abs(c_pose[1]-p[1])<delta and abs(c_pose[2]-p[2])<delta: return True else: return False if __name__ == "__main__": robot_ip = "192.168.1.200" conSuc, sock = connectETController(robot_ip) # 如果机器人有报警,清除报警 ret1, result1, id1 = sendCMD(sock, "getRobotState") if result1==4: ret, result, id = sendCMD(sock, "clearAlarm") print(ret, result) #获取同步状态 suc, Motorstatus, id = sendCMD(sock, "getMotorStatus") if Motorstatus ==0: #同步伺服编码器数据 suc, syncMotorStatus, id = sendCMD(sock, "syncMotorStatus") #设置伺服使能状态 suc, servostatus, id = sendCMD(sock, "set_servo_status", {"status": 1}) # 设置机器人运行速度百分比 suc, result , id = sendCMD(sock, "setSpeed", {"value": 10}) # 设置机器人开始位置,类型joint P000 = [-36.0662,-110.6173,98.9087,-78.2932,90.0004,53.8916] # 直线形式移动机器人到P000位置,速度类型0:线性速度, 速度100mm/s suc, result, id = sendCMD(sock,'moveByLine', {'targetPos':P000, "speed_type" :0, "speed":100}) # 等待机器人移动开始位置 wait_stop() # 获取开始位置的笛卡尔坐标 ret1, startpose, id = sendCMD(sock, "get_tcp_pose") print('startpose', startpose) # 计算绕当前末端点,参考base方向的x轴旋转15° new_pose = Rotate(sock,startpose,1,15,0,0) # startpose,参考位置, # toolorbase=1 ,绕当前base的方向,(toolorbase=0 ,绕当前TCP的方向) # rx=15,ry=0,rz=0 ,单位deg # 求逆并执行moveLine suc, new_pose_joint , id=sendCMD(sock,"inverseKinematic",{"targetPose": new_pose }) suc, result, id = sendCMD(sock,'moveByLine', {'targetPos':new_pose_joint, "speed_type" :0, "speed":100}) # 等待机器人移动开始位置 wait_stop()