SDK中实现绕末端TCP点旋转


若希望机器人绕当前TCP末端旋转啊一定角度,包括参考TCP方向或者base方向,可以使用以下代码计算并运动

import math
import socket
import json
import time
# Author:chenliao@elibot.cn

# 绕当前末端TCP点旋转,可以参考tcp方向或者base方向
def Rotate(sock,p,toolorbase=0,rx=0,ry=0,rz=0):
    # 输入参考笛卡尔空间点,后续旋转rx,ry,rz为deg
    out = [0,0,0,math.radians(rx),math.radians(ry),math.radians(rz)]
    if toolorbase==0:
        # 绕着当前TCP的方向转      
        suc, result , id=sendCMD(sock,"poseMul",{"pose1":p,"pose2":out})
    if toolorbase==1:
        # 绕着当前末端,参考base坐标系转 
        p_tmp = [0,0,0,p[3],p[4],p[5]]
        suc, result , id=sendCMD(sock,"poseMul",{"pose1":out,"pose2":p_tmp})
        result[0] =p[0]
        result[1] =p[1]
        result[2] =p[2]
    return result

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": 10})

    # 设置机器人开始位置,类型joint
    P000 = [-36.0662,-110.6173,98.9087,-78.2932,90.0004,53.8916]
    # 直线形式移动机器人到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)

    # 计算绕当前末端点,参考base方向的x轴旋转15°
    new_pose = Rotate(sock,startpose,1,15,0,0)
    # startpose,参考位置,
    # toolorbase=1 ,绕当前base的方向,(toolorbase=0 ,绕当前TCP的方向)
    # rx=15,ry=0,rz=0 ,单位deg

    # 求逆并执行moveLine
    suc, new_pose_joint , id=sendCMD(sock,"inverseKinematic",{"targetPose": new_pose })
    suc, result, id = sendCMD(sock,'moveByLine', {'targetPos':new_pose_joint, "speed_type" :0, "speed":100})
    # 等待机器人移动开始位置
    wait_stop()                   
点击显示全文
赞同0
发表评论
分享

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