可以通过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()