通过SDK外部一键启动机器人python


EC机器人处于remote模式,开机后,机器人状态如下


机器人需要完成以下动作,进入正常模式:

1. 机器人开机处于初始化状态(通过虚拟输出m473判断,1表示处于初始化状态,0表示完成初始化状态),需要使用clearAlarm指令对机器人清错和打开抱闸。机器人抱闸打开情况可以通过get_servo_brake_off_status函数判断

2.获取机器人同步状态getMotorStatus,若未同步执行同步指令syncMotorStatus

3.给机器人上使能set_servo_status

4.判断机器人是否处于精确模式(通过虚拟输出m472判断,0表示未校准模式,1表示精确模式),若未校准,执行校准操作calibrate_encoder_zero_position,并等待机器人达到m472=1的精确模式

import socket
import json
import time

# v1.2

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:
        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 WaitBrakeOpen():
    brakeopen = [0,0,0,0,0,0]
    #获取伺服抱闸打开情况
    suc, brakeopen, id = sendCMD(sock, "get_servo_brake_off_status")
    b_sum = 0

    # brakeopen为6个轴 抱闸打开情况,打开为1,不然为0,全部打开为[1,1,1,1,1,1]
    for d in brakeopen:
        b_sum = b_sum+d

    while b_sum !=6 :
        #获取伺服抱闸打开情况
        suc, brakeopen, id = sendCMD(sock, "get_servo_brake_off_status")
        b_sum = 0
        for d in brakeopen:
            b_sum = b_sum+ d
        time.sleep(0.1)    
    # 等待6个轴全部打开

if __name__ == "__main__":
    robot_ip = "192.168.1.200"
    conSuc, sock = connectETController(robot_ip)

    suc, rb_state , id = sendCMD(sock, "getRobotState")
    # 停止状态 0,暂停状态 1,急停状态 2,运行状态 3,报警状态 4,碰撞状态 5
    print('机器人状态:',rb_state)

    # 获取虚拟输出 IO 状态(初始化状态)参数: addr:虚拟 IO 地址,范围:int [400,1535]
    # m473:1表示初始化状态,0表示完成初始化状态
    suc, rb_ini_state , id=sendCMD(sock,"getVirtualOutput",{"addr":473})
    if rb_state ==4 or rb_ini_state == 1:
        #清除报警
        suc, result, id = sendCMD(sock, "clearAlarm")
        #获取伺服抱闸打开情况
        WaitBrakeOpen()

    #获取同步状态
    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, rb_calib , id=sendCMD(sock,"getVirtualOutput",{"addr":472})
    # m472为0表示未校准状态,1表示完成校准
    if rb_calib ==0:
        #编码器零位校准
        suc, result , id=sendCMD(sock, "calibrate_encoder_zero_position")
        suc, rb_calib , id=sendCMD(sock,"getVirtualOutput",{"addr":472})
        while rb_calib ==0 :
            suc, rb_calib , id=sendCMD(sock,"getVirtualOutput",{"addr":472})
            time.sleep(0.1)
   
点击显示全文
赞同0
发表评论
分享

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