透传(外部实时控制)在sdk中的使用

透传(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()
点击显示全文
赞同0
发表评论
分享

手机扫码分享
0
1.051K
收藏
举报
收起
登录
  • 密码登录
  • 验证码登录
还没有账号,立即注册
还没有账号,立即注册
注册
已有账号,立即登录
选择发帖板块
上传文件
举报
请选择举报理由
举报
举报说明