118 lines
4.4 KiB
Python
Executable File
118 lines
4.4 KiB
Python
Executable File
#! /usr/bin/env python
|
|
# coding=utf-8
|
|
|
|
"""
|
|
机械臂上电与断电
|
|
|
|
步骤:
|
|
第一步: 连接到 RPC 服务
|
|
第二步: 机械臂登录
|
|
第三步: 机械臂上电
|
|
第四步: 机械臂断电
|
|
"""
|
|
|
|
import pyaubo_sdk
|
|
import time
|
|
|
|
from scipy.spatial.transform import Rotation as R
|
|
import numpy as np
|
|
|
|
robot_ip = "192.168.100.20" # 服务器 IP 地址
|
|
robot_port = 30004 # 端口号
|
|
M_PI = 3.14159265358979323846
|
|
robot_rpc_client = pyaubo_sdk.RpcClient()
|
|
|
|
# 机械臂上电
|
|
def exampleStartup():
|
|
robot_name = robot_rpc_client.getRobotNames()[0] # 接口调用: 获取机器人的名字
|
|
if 0 == robot_rpc_client.getRobotInterface(
|
|
robot_name).getRobotManage().poweron(): # 接口调用: 发起机器人上电请求
|
|
print("The robot is requesting power-on!")
|
|
if 0 == robot_rpc_client.getRobotInterface(
|
|
robot_name).getRobotManage().startup(): # 接口调用: 发起机器人启动请求
|
|
print("The robot is requesting startup!")
|
|
# 循环直至机械臂松刹车成功
|
|
while 1:
|
|
robot_mode = robot_rpc_client.getRobotInterface(robot_name) \
|
|
.getRobotState().getRobotModeType() # 接口调用: 获取机器人的模式类型
|
|
print("Robot current mode: %s" % (robot_mode.name))
|
|
if robot_mode == pyaubo_sdk.RobotModeType.Running:
|
|
break
|
|
time.sleep(1)
|
|
|
|
# 机械臂断电
|
|
def examplePoweroff():
|
|
robot_name = robot_rpc_client.getRobotNames()[0] # 接口调用: 获取机器人的名字
|
|
if 0 == robot_rpc_client.getRobotInterface(
|
|
robot_name).getRobotManage().poweroff(): # 接口调用: 机械臂断电
|
|
print("The robot is requesting power-off!")
|
|
|
|
if __name__ == '__main__':
|
|
import math
|
|
robot_rpc_client.connect(robot_ip, robot_port) # 接口调用: 连接到 RPC 服务
|
|
if robot_rpc_client.hasConnected():
|
|
print("Robot rcp_client connected successfully!")
|
|
robot_rpc_client.login("aubo", "123456") # 接口调用: 机械臂登录
|
|
if robot_rpc_client.hasLogined():
|
|
print("Robot rcp_client logined successfully!")
|
|
exampleStartup() # 机械臂上电
|
|
# examplePoweroff() # 机械臂断电
|
|
robot_name = robot_rpc_client.getRobotNames()[0]
|
|
robot = robot_rpc_client.getRobotInterface(robot_name)
|
|
robot_config = robot.getRobotConfig()
|
|
# home_position =[math.degrees(radian) for radian in robot_config.getHomePosition()]
|
|
# print(home_position)
|
|
# offset = [0,0,0.097,0.108,0,0,0]
|
|
# while robot_config.getTcpOffset() != offset:
|
|
# robot_config.setTcpOffset(offset)
|
|
# time.sleep(1)
|
|
# print("retry")
|
|
|
|
print(robot_config.getTcpOffset())
|
|
|
|
pose = robot_rpc_client.getRobotInterface(robot_name).getRobotState().getTcpPose()
|
|
x, y, z, roll, pitch, yaw = pose # x, y, z unit: m
|
|
position = np.array([x, y, z])
|
|
quat_rot = R.from_euler('xyz', [roll, pitch, yaw], degrees=False).as_quat()
|
|
print(position,quat_rot)
|
|
print(roll, pitch, yaw)
|
|
|
|
pose = robot_rpc_client.getRobotInterface(robot_name).getRobotState().getToolPose()
|
|
x, y, z, roll, pitch, yaw = pose # x, y, z unit: m
|
|
position = np.array([x, y, z])
|
|
quat_rot = R.from_euler('xyz', [roll, pitch, yaw], degrees=False).as_quat()
|
|
print(position,quat_rot)
|
|
print(roll, pitch, yaw)
|
|
|
|
joint = robot_rpc_client.getRobotInterface(robot_name).getRobotState().getJointPositions()
|
|
print(joint)
|
|
|
|
|
|
print(robot_rpc_client.getRuntimeMachine().abort())
|
|
|
|
|
|
# sensor_mass = 0.334
|
|
# tool_mass = 0.57
|
|
# total_mass = sensor_mass + tool_mass
|
|
|
|
# mass_center_position = [-0.018592692520193167, -0.0010810233352177645, 0.11894683789633363]
|
|
|
|
# aom = [0.0, 0.0, 0.0]
|
|
# inertia = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
|
|
|
# # 调用setPayload函数设置末端负载
|
|
# while robot_config.getPayload() != (total_mass, mass_center_position, aom, inertia):
|
|
# result = robot_config.setPayload(total_mass, mass_center_position, aom, inertia)
|
|
|
|
# time.sleep(1)
|
|
# print(robot_config.getPayload())
|
|
# print("******************")
|
|
# print((total_mass, mass_center_position, aom, inertia))
|
|
# print("-----------------")
|
|
|
|
# print(robot_config.getPayload())
|
|
|
|
|
|
|
|
|