import sys
import os
os.environ["GLOG_minloglevel"] = "1"   # 屏蔽 INFO
# from python.vision_servo.follow_target import moveJ
import time

# 把.so所在目录加入 Python 路径
sys.path.append("/home/lgv/cmvr/cmvr-es/cmake-build-debug/example")

# 导入模块
from robot_wrapper import Robot

# R_WRIST_R_S
# 初始化机器人（传入配置文件路径和机器人名称）
robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")

# robot.torqueOn()
#
#
# x,y,z,rx,ry,rz = robot.fk(base_link="PELVIS_S", ee_link="R_FINGER_TIP")
# # robot->move(base_link="PELVIS_S", target_link="R_FINGER_TIP")
#
#
# print(x, y, z, rx, ry, rz, sep=',')
#
#
# while True:
#     for j in JOINT_POSITIONS:
#         robot.moveJ('right', j)

# time.sleep(10)
# js = robot.getJointQ('right')
# print(js)
# 控制左臂关节
robot.calibrateZeroQ("R_WRIST_Y")
# robot.calibrateZeroQ("R_WRIST_R")
time.sleep(3)
# robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
# # robot.moveJ("right", [-0.344938,  0.935147, 2.27031,1.68959, -2.32841,0.460145, 0.300996])
# robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
# #
# time.sleep(10)

# robot.torqueOff("R_WRIST_R")
# time.sleep(10)
# robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
# time.sleep(20)
# # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364])
# robot.torqueOn()
# # time.sleep(500)
# robot.torqueOff("WAIST_P")
# time.sleep(30)
# //     {"R_SHOULDER_P", 0.00203898},
# //     {"R_SHOULDER_R", 1.34062},
# //     {"R_SHOULDER_Y", 0.0},
# //     {"R_ELBOW_R",    0.522261},
# //     {"R_WRIST_P",    0.0},
# //     {"R_WRIST_Y",    -0.000210733},
# //     {"R_WRIST_R",    -0.0942364}
# 控制右臂关节
# robot.moveJ("right", [0.1, 0.4, -0.3, 0.2, 0.0, -0.1, 0.2])