节卡机械臂末端位置信息

节卡机械臂末端位置信息

本代码用于实时保存节卡机械臂末端坐标（包括空间坐标x、y、z，法向量rx、ry、rz）。
在代码中对应的数组为：ret[1][18]

# -*- coding: utf-8 -*-
import sys
import time
import jkrc
import numpy as np

robot = jkrc.RC("10.5.5.100")  # 返回一个机器人对象
robot.login()
robot.power_on()
robot.enable_robot()
PI=3.1415926
def print_state(name,robot):#查询机器人运行状态
    while(1):
        ret = robot.get_program_state() #查询程序运行状态，0 程序终止或无程序运行，1 程序运行中，2 暂停
        print("the robot program state is:",ret[1])
        time.sleep(1)

ret = robot.program_load("Sxingquxian")#加载通过APP编写的脚本 program_test需要自己编写
ret = robot.get_loaded_program()
print("the loaded program is:",ret[1])
robot.program_run()
time.sleep(4)#机械臂运动到起始位置的时间内不进行坐标输出

for i in range(1,100):#持续输出TCP坐标
    ret = robot.get_robot_status()#由于机械臂新旧版本解释器问题，采用获得机械臂状态指令来替代机械臂的TCP坐标获取指令
    if ret[0] == 0:
        print("the tcp position is :", ret[1][18])
        #print(type(ret[1][19][1]))
        with open('test.txt', 'ab') as f:#将TCP坐标写入TXT
            np.savetxt(f, ret[1][18], delimiter=',', fmt='%.18f')
    else:
            print("some things happend,the errcode is: ", ret[0])
    time.sleep(1)#写入TCP坐标的时间间隔
# robot.disable_robot()
# robot.power_off()
robot.logout()  # 登出