| from pathlib import Path
|
| import numpy as np
|
| from dobot_control.dynamixel.driver import DynamixelDriver
|
| MENAGERIE_ROOT: Path = Path(__file__).parent / "third_party" / "mujoco_menagerie"
|
| from scripts.manipulate_utils import load_ini_data_hands
|
| from scripts.function_util import scan_port
|
|
|
|
|
| def get_config(which_hand, which_hand_config):
|
| gripper_ids = {"HAND_LEFT": [8], "HAND_RIGHT": [18]}
|
| driver =DynamixelDriver(ids=which_hand_config.joint_ids+gripper_ids[which_hand],
|
| append_id=which_hand_config.append_id,
|
| port=which_hand_config.port, baudrate=hands_dict[which_hand].baud_rate)
|
|
|
| print("--------------------", which_hand, "-------------------")
|
| pos_joint = driver.get_joints()
|
| curr_joints = pos_joint[:6]
|
| print("curr_joints: ", curr_joints)
|
| print("robot_joints: ", which_hand_config.start_joints)
|
|
|
| dev_pos = [float("%.2f" % (curr_joints[i]-which_hand_config.start_joints[i]*which_hand_config.joint_signs[i]))
|
| for i in range(6)]
|
| print("dev(write): ", dev_pos)
|
| print("dev(*pi/2): ", [(i / np.pi) * 2 for i in curr_joints])
|
| print("dev(angle): ", [np.rad2deg(i) for i in curr_joints])
|
| print("----------------------------------------------")
|
| gripper_on = int(np.rad2deg(pos_joint[-1]) - 0.2)
|
| gripper_close = int(np.rad2deg(pos_joint[-1]) + 30)
|
| print(
|
| "gripper open (degrees) ",
|
| gripper_on,
|
| )
|
| print(
|
| "gripper close (degrees) ",
|
| gripper_close,
|
| )
|
| return dev_pos, [gripper_ids[which_hand][0], gripper_close, gripper_on]
|
|
|
|
|
| if __name__ == "__main__":
|
|
|
| scan_port()
|
| ini_file_path = str(Path(__file__).parent) + "/dobot_config/dobot_settings.ini"
|
| ini_file, hands_dict = load_ini_data_hands()
|
| for _hand in hands_dict.keys():
|
| offsets, pos_gripper = get_config(_hand, hands_dict[_hand])
|
| ini_file.set(section=_hand, option="joint_offsets",
|
| value=str(offsets).replace("[", '').replace("]", ''))
|
| ini_file.set(section=_hand, option="gripper_config",
|
| value=str(pos_gripper).replace("[", '').replace("]", ''))
|
| with open(ini_file_path, "w+") as _file:
|
| ini_file.write(_file)
|
| _file.close()
|
|
|
|
|
