"""双臂位置运动;以各自当前姿态作为目标."""

from libark.robot.hw1.arm import DualArm


def main() -> None:
    with DualArm() as arms:
        arms.enable()
        state = arms.get_state()
        if not state.left.ok or not state.right.ok:
            raise RuntimeError("双臂反馈不完整,请联系技术支持")
        left, right = list(state.left.q7), list(state.right.q7)
        arms.move_joints(left, right, speed_percent=20)
        print("左臂:", arms.left.get_state().pos)
        print("右臂:", arms.right.get_state().pos)


if __name__ == "__main__":
    main()