SDK实现机器人绘制正方形

可以通过SDK控制机器人运动。

控制机器人移动的指令包括moveByJoint, moveByLine,以及moveByPath等。

以上三种指令传入的点位均为joint类型,若需要使用xyz的笛卡尔坐标,需要通过inverseKinematic求逆

moveByPath可以批量发送点位,且使用圆滑效果。

具体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 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 Offs(p, x, y, z):
    # 偏移函数
    p1 = p.copy()
    p1[0] = p1[0]+x
    p1[1] = p1[1]+y
    p1[2] = p1[2]+z
    return p1

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})

    # 设置机器人开始位置,类型joint
    P000 = [-36.0662, -110.6173, 98.9087, -78.2932, 90.0004, 53.8916]
    # MoveJ形式移动机器人到P001位置,速度为百分比
    suc, result, id = sendCMD(sock, 'moveByJoint', {
                              'targetPos': P000, 'speed': 10})
    # 等待机器人移动开始位置
    wait_stop()

    # 获取开始位置的笛卡尔坐标
    ret1, startpose, id = sendCMD(sock, "get_tcp_pose")
    print('startpose', startpose)

    # 通过Offs函数,计算得到新的目标点,并通过inverseKinematic计算得到对应的逆解(joint)
    suc, P1 , id=sendCMD(sock,"inverseKinematic",{"targetPose": Offs(startpose,50,0,0)})
    suc, P2 , id=sendCMD(sock,"inverseKinematic",{"targetPose": Offs(startpose,50,50,0)})
    suc, P3 , id=sendCMD(sock,"inverseKinematic",{"targetPose": Offs(startpose,0,50,0)})
    suc, P4 , id=sendCMD(sock,"inverseKinematic",{"targetPose": Offs(startpose,0,0,0)})

    # 使用moveByLine/moveByJoint指令时,不能设置点与点之间的圆滑,机器人会精确走到每个点
    suc, result, id = sendCMD(sock, 'moveByLine', {'targetPos': P1, "speed_type": 0, "speed": 100})
    wait_stop()
    suc, result, id = sendCMD(sock, 'moveByLine', {'targetPos': P2, "speed_type": 0, "speed": 100})
    wait_stop()
    suc, result, id = sendCMD(sock, 'moveByLine', {'targetPos': P3, "speed_type": 0, "speed": 100})
    wait_stop()
    suc, result, id = sendCMD(sock, 'moveByLine', {'targetPos': P4, "speed_type": 0, "speed": 100})
    wait_stop()
  
    # 先将之前的轨迹点位清除
    suc, result , id = sendCMD(sock, "clearPathPoint")
    # 使用运动2.0指令,使用addPathPoint添加点位,可以设置每一句的运动模式和转弯半径等参数,达到轨迹圆滑效果
    # 先将需要运动的所有点(joint类型)发送给机器人。
    suc, result , id = sendCMD(sock, "addPathPoint", {"wayPoint": P1,"moveType": 1, "speed":100, "circular_radius":20})
    suc, result , id = sendCMD(sock, "addPathPoint", {"wayPoint": P2,"moveType": 1, "speed":100, "circular_radius":20})
    suc, result , id = sendCMD(sock, "addPathPoint", {"wayPoint": P3,"moveType": 1, "speed":100, "circular_radius":20})
    suc, result , id = sendCMD(sock, "addPathPoint", {"wayPoint": P4,"moveType": 1, "speed":100, "circular_radius":20})
    # 开始移动
    suc, result , id = sendCMD(sock, "moveByPath")
    wait_stop()
点击显示全文
赞同0
发表评论
分享

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