SDK中时间戳TrajectoryService的使用


时间戳TrajectoryService为EC SDK中向用户提供的高级功能。

用户可以离线做轨迹规划,生成带时间戳的轨迹点(joint或者笛卡尔坐标)。时间戳最小粒度可达1ms

例如要实现以下轨迹,其中用户自行规划圆的轨迹(生成带时间戳的点位轨迹,例如基于ROS等)

带时间戳的轨迹调整速度方式:缩小时间戳间隔,增加机器人整体百分比速度,更改开始时间戳运动speed_percent的参数

python上位机示例代码如下:


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 sendCMD2(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"))
        return (True, True, id)
    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]
    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)
    # 获取开始位置的joint,作为时间戳运动第一个笛卡尔空间点的逆解参考点
    ret1, startpos, id = sendCMD(sock, "get_joint_pos")    
    print('开始角度', startpos)   
  
    # 参考坐标系
    frame = [0, 0, 0, 0, 0, 0]
    suc , result , id = sendCMD(sock , "flush_trajectory")
    # 清空队列
    suc, result , id=sendCMD(sock,"start_push_pos",{"path_lenth" :360, "pos_type" :1, "ref_joint_pos":startpos,"ref_frame":frame,"ret_flag":1})
    # path_length: 轨迹点位长度
    # pos_type: 1表示笛卡尔,0表示joint
    # ref_joint_pos: 如果pos_type为1,此数据为第一个笛卡尔空间点的逆解参考解,如果pos_type为0,此数据依旧需要为[0,0,0,0,0,0]形式,不可或缺
    # ref_frame: 如果pos_type为1,此数据为所有笛卡尔空间点的参考坐标系,如果pos_type为0,此数据依旧需要为[0,0,0,0,0,0]形式,不可或缺
    # ret_flag: 1表示使用push_pos的时候,每次添加点,会有返回结果
    #           如果添加的点位较多,建议设为0,即添加后无返回确认

    r = 50
    startpose[0] = startpose[0] +r
    Pout = startpose.copy()
    # 
    
    time_stamp = 0
    for i in range(0,360):
        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 , "push_pos", {"timestamp":time_stamp , "pos": Pout })
        # 如果在start_push_pos中设置ret_flag=0,此处可以使用sendCMD2,即添加点后无返回值
        # suc , result , id = sendCMD2(sock , "push_pos", {"timestamp":time_stamp , "pos": Pout })
        print(i,result)
        time_stamp = time_stamp+0.002
        # 第一个点的时间戳必须为0.0
        # 时间戳间隔20ms,此处例如0,0.002,0.004,0.006s
        # 可以通过调节时间戳,改变机器人速度
    
    suc , result , id = sendCMD(sock , "stop_push_pos")
    print(result)
    suc , result , id = sendCMD(sock , "check_trajectory")
    print(result)
    suc , result , id = sendCMD(sock , "start_trajectory", {"speed_percent": 100})
    wait_stop()
    suc , result , id = sendCMD(sock , "flush_trajectory")

    suc, result, id = sendCMD(sock,'moveByLine', {'targetPos':P001, "speed_type" :0, "speed":100})
    # 等待机器人移动开始位置
    wait_stop()

点击显示全文
赞同0
发表评论
分享

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