import math
from utils import RobotClient

HOST = "192.168.3.2"
PORT = 9090

config = {
    "pre_grasp_position": {
        'pos': [0.14726859839859208, 0.5302551637478977, 0.565792932407347],
        'ori': [0.1305294928324298, -0.34660161091966263, 0.8293216816453197, 0.4183956538514503]
    },
    "grasp_position": {
        'pos': [0.15928907070579643, 0.5270829242433969, 0.6510703359003733],
        'ori': [0.15023706706737835, -0.32373703396446113, 0.8482916258752815, 0.39118342754871005]
    },
    "post_grasp_position": {
        'pos': [0.08296640360052251, 0.5270826520836475, 0.4622718470958212],
        'ori': [0.15024595915643826, -0.3237308590654784, 0.8482945564918141, 0.3911787674097941]
    },
    "pre_insert_position": {
        'pos': [0.08296640360052251, 0.5270826520836475, 0.4622718470958212],
        'ori': [0.15024595915643826, -0.3237308590654784, 0.8482945564918141, 0.3911787674097941]
    },
    "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(2)
    robot.set_joint_maxvelc(2)
#    robot.move_linear_cartesian(config["pre_grasp_position"])
    robot.move_to_pose(config["pre_grasp_position"])
#    robot.move_to_pose(config["grasp_position"])
#    robot.move_to_pose(config["post_grasp_position"])
    robot.close()



if __name__ == "__main__":
    main()