import sys

# 把.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")


robot.torqueOff()
# 控制左臂关节
# robot.moveJ("left", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
# robot.moveJ("left", [0.0, -1.31873, 0.0, -0.337621, 0.0,0.000183204, 0.0225797])



# 控制右臂关节
# robot.moveJ("right", [0.1, 0.4, -0.3, 0.2, 0.0, -0.1, 0.2])