62 lines
1.8 KiB
Python
62 lines
1.8 KiB
Python
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_R")
|
|
robot.calibrateZeroQ("R_WRIST_Y")
|
|
time.sleep(1)
|
|
print("--------------------------")
|
|
# 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]) |