import math
from utils import RobotClient

HOST = "192.168.3.1"
PORT = 9090

config = {
    "pre_grasp_position": {
        "pos": [0.5, 0.0, 0.3],  # в метрах
        "ori": [1.0, 0, 0, 0],  # quat wxyz
    },
    "grasp_position": {
        "pos": [0.5, 0.0, 0.3],
        "ori": [0, 0, 0, 0],  
    },
    "post_grasp_position": {
        "pos": [0.5, 0.0, 0.3],
        "ori": [0, 0, 0],
    },
    "pre_insert_position": {
        "pos": [0.6, 0.0, 0.2],
        "ori_offset": [10, -5, 0],  # углы отклонения от вертикали
    },
    "insert_position": {
        "pos": [0.6, 0.0, 0.2],
        "ori_offset": [10, -5, 0], 
    },
    "post_insert_position": {
        "pos": [0.6, 0.0, 0.2],
        "ori_offset": [10, -5, 0],
    },
}




def main():
    robot = RobotClient(HOST, PORT)
    robot.set_joint_maxacc(1.5)
    robot.set_joint_maxvelc(1.5)
    robot.move_linear_cartesian(config["pre_grasp_position"])
    robot.close()



if __name__ == "__main__":
    main()