透传(transparent transformation)是艾利特机器人EC系列中高阶的运动指令接口,允许用户自己设计并运行一段固定或者实时变化的路径。这项功能使用户可以完全控制运动路径的插补方式与运动速度,也可以在搭载了传感器和控制器的情况下实时控制机器人的运动,为用户在运动控制部分提供了较大的自由度。
例如,希望机器人从P001走到P000,并按照上图画一个圆(圆的点位由上位机实时计算),返回P000,最后回到P001,可以使用以下的代码。
SDK示例代码中的相关透传术语解释如下图
import math import socket import json import time # Author:chenliao@elibot.cn 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": 100}) # 设置机器人开始位置,类型joint P000 = [-36.0662,-110.6173,98.9087,-78.2932,90.0004,53.8916] P001 = [-36.0659,-109.1445,91.6433,-72.5008,90.0004,53.8916] # MoveJ形式移动机器人到P001位置,速度为百分比 suc, result, id = sendCMD(sock,'moveByJoint', {'targetPos':P001, 'speed':10}) # 等待机器人移动到位 wait_stop() # 直线形式移动机器人到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) suc, result, id = sendCMD(sock, 'get_transparent_transmission_state') if result ==1: # 如果处于透传状态,先清空之前的队列数据 suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") time.sleep(0.5) # tt:transparent transmission: 透传 # 设置透传的lookahead时间为200ms # 设置透传的采样时间为20ms, # 即机器人会每20ms把上位机发来的最后一次数据存入机器人tt队列的最后一个数据,并剔除tt队列的第一个数据 # 即透传队列长度为200/20 = 10 # 设置透传平滑度为1,范围0-1 lookahead_time =200 #ms sample_time = 20 #ms smoothess = 1 #范围0-1 suc, result, id = sendCMD(sock, "transparent_transmission_init", {"lookahead": lookahead_time, "t": sample_time, "smoothness": smoothess}) # 机器人在开始透传运动的时候,会先将当前位置全部填入tt的运动队列 # 即机器人收到新的数据,新的数据会存入tt队列最后一个并剔除第一个数据,机器人就会运动 r = 50 #画半径为50mm的圆 startpose[0] = startpose[0] +r Pout =startpose.copy() step = 1 # 每1°发一个点 # 可以调节点之间的距离,变相调节机器人速度 for i in range(0,360,step): Pout[0] = startpose[0] - r * math.cos(math.radians(i)) Pout[1] = startpose[1] + r * math.sin(math.radians(i)) suc, result, id = sendCMD(sock, "tt_put_servo_joint_to_buf", {"targetPose": Pout}) # 发送的数据类型是笛卡尔的xyzabc类型Pose # 也可以使用sendCMD(sock, "tt_put_servo_joint_to_buf", {"targetPos": Pout})发送joint类型数据 time.sleep(0.02) # 延时20ms # 通过调节该延时,达到调整机器人速度 while checkPose(sock,Pout,2)==False: # 如果机器人没有走到透传的最后的目标点,一直发送最后一个点 suc, result, id = sendCMD(sock, "tt_put_servo_joint_to_buf", {"targetPose": Pout}) time.sleep(0.02) suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") wait_stop() suc, result, id = sendCMD(sock,'moveByLine', {'targetPos':P001, "speed_type" :0, "speed":100}) # 等待机器人移动到位 wait_stop()