98 lines
3.3 KiB
Python
98 lines
3.3 KiB
Python
from pathlib import Path
|
|
|
|
### Simulation envs fixed constants
|
|
DT = 0.02
|
|
JOINT_NAMES = ["waist", "shoulder", "elbow", "forearm_roll", "wrist_angle", "wrist_rotate"]
|
|
START_ARM_POSE = [
|
|
0,
|
|
-0.96,
|
|
1.16,
|
|
0,
|
|
-0.3,
|
|
0,
|
|
0.02239,
|
|
-0.02239,
|
|
0,
|
|
-0.96,
|
|
1.16,
|
|
0,
|
|
-0.3,
|
|
0,
|
|
0.02239,
|
|
-0.02239,
|
|
]
|
|
|
|
ASSETS_DIR = Path(__file__).parent.resolve() / "assets" # note: absolute path
|
|
|
|
# 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
|