#! /usr/bin/python3

from UR10eArm import UR10eArm
# from ArmGripperComm import ArmGripperComm
import rospy

if __name__ == '__main__':
    rospy.init_node("move_group_python_interface", anonymous=True)
    # --------------------------------------------------------------------
    # -------------------------initialization-----------------------------
    # --------------------------------------------------------------------

    # arm_gripper_comm = ArmGripperComm()
    # arm_gripper_comm.move_arm_to_initial_pose()


    arm = UR10eArm()

    state = arm.go_to_joint_state(0.01725006103515625, -1.9415461025633753, 1.8129728476153772, -1.5927173099913539,
                                  -1.5878670851336878, 0.03150486946105957, 1, 1)

    state = arm.go_to_joint_state(-0.08442861238588506, -0.9531238240054627, 1.1278675238238733, -2.4318977795042933,
                                  -1.5508225599872034, 0.026221752166748047, 1, 1)
    #
    # current_pose = move_group.get_current_pose().pose
    # print("current_pose")
    # print(current_pose)
    #
    # current_joints = move_group.get_current_joint_values()
    # print("current_joints")
    # print(current_joints)