cmvr-es/python/vision_servo/robot_warpper_test.py

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

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





#
#
# while True:
#     for j in JOINT_POSITIONS:
#         robot.moveJ('right', j)
# robot.torqueOn()
# 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])