65 lines
3.4 KiB
Python
65 lines
3.4 KiB
Python
import pathlib
|
|
|
|
### Task parameters
|
|
|
|
HOME_PATH = str(pathlib.Path(__file__).parent.resolve())
|
|
|
|
DATASET_DIR = HOME_PATH + '/../demos/dataset'
|
|
|
|
CONFIG_DIR = HOME_PATH + '/../demos/config.yaml'
|
|
|
|
SIM_TASK_CONFIGS = {
|
|
# 'sim_insertion': {
|
|
# 'dataset_dir': DATASET_DIR + '/sim_insertion',
|
|
# 'num_episodes': 7,
|
|
# 'episode_len': 1950,
|
|
# 'camera_names': ['top','angle'],
|
|
# 'xml_dir': HOME_PATH + '/assets'
|
|
# },
|
|
'sim_transfer': {
|
|
'dataset_dir': DATASET_DIR + '/sim_transfer',
|
|
'num_episodes': 20,
|
|
'episode_len': 700,
|
|
'camera_names': ['top','r_vis','front'],
|
|
'xml_dir': HOME_PATH + '/assets'
|
|
},
|
|
|
|
}
|
|
|
|
|
|
# Left finger position limits (qpos[7]), right_finger = -1 * left_finger
|
|
MASTER_GRIPPER_POSITION_OPEN = 0.02417
|
|
MASTER_GRIPPER_POSITION_CLOSE = 0.01244
|
|
PUPPET_GRIPPER_POSITION_OPEN = 0.05800
|
|
PUPPET_GRIPPER_POSITION_CLOSE = 0.01844
|
|
|
|
# Gripper joint limits (qpos[6])
|
|
MASTER_GRIPPER_JOINT_OPEN = 0.3083
|
|
MASTER_GRIPPER_JOINT_CLOSE = -0.6842
|
|
PUPPET_GRIPPER_JOINT_OPEN = 1.4910
|
|
PUPPET_GRIPPER_JOINT_CLOSE = -0.6213
|
|
|
|
############################ Helper functions ############################
|
|
|
|
MASTER_GRIPPER_POSITION_NORMALIZE_FN = lambda x: (x - MASTER_GRIPPER_POSITION_CLOSE) / (MASTER_GRIPPER_POSITION_OPEN - MASTER_GRIPPER_POSITION_CLOSE)
|
|
PUPPET_GRIPPER_POSITION_NORMALIZE_FN = lambda x: (x - PUPPET_GRIPPER_POSITION_CLOSE) / (PUPPET_GRIPPER_POSITION_OPEN - PUPPET_GRIPPER_POSITION_CLOSE)
|
|
MASTER_GRIPPER_POSITION_UNNORMALIZE_FN = lambda x: x * (MASTER_GRIPPER_POSITION_OPEN - MASTER_GRIPPER_POSITION_CLOSE) + MASTER_GRIPPER_POSITION_CLOSE
|
|
PUPPET_GRIPPER_POSITION_UNNORMALIZE_FN = lambda x: x * (PUPPET_GRIPPER_POSITION_OPEN - PUPPET_GRIPPER_POSITION_CLOSE) + PUPPET_GRIPPER_POSITION_CLOSE
|
|
MASTER2PUPPET_POSITION_FN = lambda x: PUPPET_GRIPPER_POSITION_UNNORMALIZE_FN(MASTER_GRIPPER_POSITION_NORMALIZE_FN(x))
|
|
|
|
MASTER_GRIPPER_JOINT_NORMALIZE_FN = lambda x: (x - MASTER_GRIPPER_JOINT_CLOSE) / (MASTER_GRIPPER_JOINT_OPEN - MASTER_GRIPPER_JOINT_CLOSE)
|
|
PUPPET_GRIPPER_JOINT_NORMALIZE_FN = lambda x: (x - PUPPET_GRIPPER_JOINT_CLOSE) / (PUPPET_GRIPPER_JOINT_OPEN - PUPPET_GRIPPER_JOINT_CLOSE)
|
|
MASTER_GRIPPER_JOINT_UNNORMALIZE_FN = lambda x: x * (MASTER_GRIPPER_JOINT_OPEN - MASTER_GRIPPER_JOINT_CLOSE) + MASTER_GRIPPER_JOINT_CLOSE
|
|
PUPPET_GRIPPER_JOINT_UNNORMALIZE_FN = lambda x: x * (PUPPET_GRIPPER_JOINT_OPEN - PUPPET_GRIPPER_JOINT_CLOSE) + PUPPET_GRIPPER_JOINT_CLOSE
|
|
MASTER2PUPPET_JOINT_FN = lambda x: PUPPET_GRIPPER_JOINT_UNNORMALIZE_FN(MASTER_GRIPPER_JOINT_NORMALIZE_FN(x))
|
|
|
|
MASTER_GRIPPER_VELOCITY_NORMALIZE_FN = lambda x: x / (MASTER_GRIPPER_POSITION_OPEN - MASTER_GRIPPER_POSITION_CLOSE)
|
|
PUPPET_GRIPPER_VELOCITY_NORMALIZE_FN = lambda x: x / (PUPPET_GRIPPER_POSITION_OPEN - PUPPET_GRIPPER_POSITION_CLOSE)
|
|
|
|
MASTER_POS2JOINT = lambda x: MASTER_GRIPPER_POSITION_NORMALIZE_FN(x) * (MASTER_GRIPPER_JOINT_OPEN - MASTER_GRIPPER_JOINT_CLOSE) + MASTER_GRIPPER_JOINT_CLOSE
|
|
MASTER_JOINT2POS = lambda x: MASTER_GRIPPER_POSITION_UNNORMALIZE_FN((x - MASTER_GRIPPER_JOINT_CLOSE) / (MASTER_GRIPPER_JOINT_OPEN - MASTER_GRIPPER_JOINT_CLOSE))
|
|
PUPPET_POS2JOINT = lambda x: PUPPET_GRIPPER_POSITION_NORMALIZE_FN(x) * (PUPPET_GRIPPER_JOINT_OPEN - PUPPET_GRIPPER_JOINT_CLOSE) + PUPPET_GRIPPER_JOINT_CLOSE
|
|
PUPPET_JOINT2POS = lambda x: PUPPET_GRIPPER_POSITION_UNNORMALIZE_FN((x - PUPPET_GRIPPER_JOINT_CLOSE) / (PUPPET_GRIPPER_JOINT_OPEN - PUPPET_GRIPPER_JOINT_CLOSE))
|
|
|
|
MASTER_GRIPPER_JOINT_MID = (MASTER_GRIPPER_JOINT_OPEN + MASTER_GRIPPER_JOINT_CLOSE)/2
|