# import time
#
# from hand_client import DexHandClient
#
#
# from movej_client import MoveJClient
#
# import atexit
#
# hand_open = [
#         {"id": 0, "value": 0.8},
#         {"id": 1, "value": 0.8},
#         {"id": 2, "value": 0.8},
#         {"id": 3, "value": 0.8},
#         {"id": 4, "value": 0.8},
#         {"id": 5, "value": 1.0},
#     ]
#
#
# hand_touch = [
#     {"id": 0, "value": 0},
#     {"id": 1, "value": 0},
#     {"id": 2, "value": 0},
#     {"id": 3, "value": 1.0},
#     {"id": 4, "value": 0.3},
#     {"id": 5, "value": 0.3},
# ]
#
# handshake = [
#         {"id": 0, "value": 0.0},
#         {"id": 1, "value": 0.0},
#         {"id": 2, "value": 0.0},
#         {"id": 3, "value": 0.0},
#         {"id": 4, "value": 0.0},
#         {"id": 5, "value": 1.0},
#     ]
#
#
# left_joint_list = [
#
#     {'joint_name': 'L_SHOULDER_P', 'rad': -0.230949540675},
#     {'joint_name': 'L_SHOULDER_R', 'rad': -1.0384195613},
#     {'joint_name': 'L_SHOULDER_Y', 'rad': -15.045008286377},
#     {'joint_name': 'L_ELBOW_R', 'rad': -1.615503893099},
#     {'joint_name': 'L_WRIST_P', 'rad': 14.787740913187},
#     {'joint_name': 'L_WRIST_Y', 'rad': -0.353138324696},
#     {'joint_name': 'L_WRIST_R', 'rad': 0.28},
#     ]
#
# right_joint_list = [
#
# {'joint_name': 'L_SHOULDER_P', 'rad': -0.038843127376},
# {'joint_name': 'L_SHOULDER_R', 'rad': -0.782479233371},
# {'joint_name': 'L_SHOULDER_Y', 'rad': -14.639593111463},
# {'joint_name': 'L_ELBOW_R', 'rad': -2.009983759425},
# {'joint_name': 'L_WRIST_P', 'rad': 14.79076046324},
# {'joint_name': 'L_WRIST_Y', 'rad': -0.022087994402},
# {'joint_name': 'L_WRIST_R', 'rad': 0.28},
#
#
# ]
# start_joint_list = [
#     {'joint_name': 'L_SHOULDER_P', 'rad': -0.029367758425},
#     {'joint_name': 'L_SHOULDER_R', 'rad': -1.448823970756},
#     {'joint_name': 'L_SHOULDER_Y', 'rad': -14.5604203382},
#     {'joint_name': 'L_ELBOW_R', 'rad': -0.184456443516},
#     {'joint_name': 'L_WRIST_P', 'rad': 14.629440930445},
#     {'joint_name': 'L_WRIST_Y', 'rad': -0.021720636379},
#     {'joint_name': 'L_WRIST_R', 'rad': -0.120802885537},
#     ]
#
# single_joint = [
#         {'joint_name': 'R_SHOULDER_Y', 'rad': 0},
#     ]
#
#
# first_joint_list = [
# {'joint_name': 'L_SHOULDER_P', 'rad': 0.013117624077},
# {'joint_name': 'L_SHOULDER_R', 'rad': -1.153729162914},
# {'joint_name': 'L_SHOULDER_Y', 'rad': -14.255272070875},
# {'joint_name': 'L_ELBOW_R', 'rad': -1.477644963752},
# {'joint_name': 'L_WRIST_P', 'rad': 15.28202635377},
# {'joint_name': 'L_WRIST_Y', 'rad': -0.000359764059},
# {'joint_name': 'L_WRIST_R', 'rad': 0.124166062475},
# ]
#
# second_joint_list=[
# {'joint_name': 'L_SHOULDER_P', 'rad': -0.563525308548},
# {'joint_name': 'L_SHOULDER_R', 'rad': -1.290596130675},
# {'joint_name': 'L_SHOULDER_Y', 'rad': -14.162486167364},
# {'joint_name': 'L_ELBOW_R', 'rad': -1.021621712276},
# {'joint_name': 'L_WRIST_P', 'rad': 15.432362166456},
# {'joint_name': 'L_WRIST_Y', 'rad': 0.018426754365},
# {'joint_name': 'L_WRIST_R', 'rad': -0.005352795583},
# ]
#
# def action_hello():
#     # client.send(start_joint_list, vel=2.0, acc=2.0)
#     hand_client.set_angles(hand_open, device_id="hand1")
#     client.send(right_joint_list, vel=2.0, acc=2.0)
#     # time.sleep(1)
#     while True:
#         client.send(right_joint_list, vel=3.0, acc=3.0)
#         client.send(left_joint_list, vel=3.0, acc=3.0)
#     # client.send(start_joint_list, vel=1.0, acc=1.0)
#
# def action_touch():
#     # client.send(start_joint_list, vel=2.0, acc=2.0)
#     hand_client.set_angles(hand_touch, device_id="hand1")
#     client.send(first_joint_list, vel=2.0, acc=2.0)
#     # time.sleep(1)
#     while True:
#         client.send(first_joint_list, vel=5.0, acc=3.0)
#         client.send(second_joint_list, vel=5.0, acc=3.0)
#     # client.send(start_joint_list, vel=1.0, acc=1.0)
#
#
# def cleanup():
#     time.sleep(3)
#     hand_client.set_angles(handshake, device_id="hand1")
#     client.send(start_joint_list, vel=2.0, acc=2.0)
#     if hand_client is not None:
#         hand_client.close()
#     if client is not None:
#         client.close()
#     print("程序结束：已执行清理并打印此信息。")
#
# atexit.register(cleanup)
#
#
# if __name__ == "__main__":
#     # Initialize client
#     global hand_client
#     global client
#
#     hand_client = DexHandClient()
#     client = MoveJClient()
#
#     action_touch()
#
#