#! /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())