UPDATE emancro gello
This commit is contained in:
Yuwei Xia
parent
daae81f4a7
commit
8e84ad0222
6 changed files with 153 additions and 64 deletions
|
|
@ -29,9 +29,9 @@ class DynamixelRobotConfig:
|
|||
assert len(self.joint_ids) == len(self.joint_offsets)
|
||||
assert len(self.joint_ids) == len(self.joint_signs)
|
||||
|
||||
def make_robot(
|
||||
self, port: str = "/dev/ttyUSB0", start_joints: Optional[np.ndarray] = None
|
||||
) -> DynamixelRobot:
|
||||
def make_robot(self,
|
||||
port: str = "/dev/ttyUSB0",
|
||||
start_joints: Optional[np.ndarray] = None) -> DynamixelRobot:
|
||||
return DynamixelRobot(
|
||||
joint_ids=self.joint_ids,
|
||||
joint_offsets=list(self.joint_offsets),
|
||||
|
|
@ -45,68 +45,73 @@ class DynamixelRobotConfig:
|
|||
|
||||
PORT_CONFIG_MAP: Dict[str, DynamixelRobotConfig] = {
|
||||
# xArm
|
||||
# "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT3M9NVB-if00-port0": DynamixelRobotConfig(
|
||||
# joint_ids=(1, 2, 3, 4, 5, 6, 7),
|
||||
# joint_offsets=(
|
||||
# 2 * np.pi / 2,
|
||||
# 2 * np.pi / 2,
|
||||
# 2 * np.pi / 2,
|
||||
# 2 * np.pi / 2,
|
||||
# -1 * np.pi / 2 + 2 * np.pi,
|
||||
# 1 * np.pi / 2,
|
||||
# 1 * np.pi / 2,
|
||||
# ),
|
||||
# joint_signs=(1, 1, 1, 1, 1, 1, 1),
|
||||
# gripper_config=(8, 279, 279 - 50),
|
||||
# ),
|
||||
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT94VP8U-if00-port0":
|
||||
DynamixelRobotConfig(
|
||||
joint_ids=(1, 2, 3, 4, 5, 6, 7),
|
||||
joint_offsets=(
|
||||
1 * np.pi / 2,
|
||||
1 * np.pi / 2,
|
||||
1 * np.pi / 2,
|
||||
1 * np.pi / 2,
|
||||
1 * np.pi / 2,
|
||||
1 * np.pi / 2,
|
||||
1 * np.pi / 2,
|
||||
),
|
||||
joint_signs=(1, 1, 1, 1, 1, 1, 1),
|
||||
gripper_config=(8, 115.024609375, 73.224609375),
|
||||
),
|
||||
# panda
|
||||
# "/dev/cu.usbserial-FT3M9NVB": DynamixelRobotConfig(
|
||||
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT3M9NVB-if00-port0": DynamixelRobotConfig(
|
||||
joint_ids=(1, 2, 3, 4, 5, 6, 7),
|
||||
joint_offsets=(
|
||||
3 * np.pi / 2,
|
||||
2 * np.pi / 2,
|
||||
1 * np.pi / 2,
|
||||
4 * np.pi / 2,
|
||||
-2 * np.pi / 2 + 2 * np.pi,
|
||||
3 * np.pi / 2,
|
||||
4 * np.pi / 2,
|
||||
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT3M9NVB-if00-port0":
|
||||
DynamixelRobotConfig(
|
||||
joint_ids=(1, 2, 3, 4, 5, 6, 7),
|
||||
joint_offsets=(
|
||||
3 * np.pi / 2,
|
||||
2 * np.pi / 2,
|
||||
1 * np.pi / 2,
|
||||
4 * np.pi / 2,
|
||||
-2 * np.pi / 2 + 2 * np.pi,
|
||||
3 * np.pi / 2,
|
||||
4 * np.pi / 2,
|
||||
),
|
||||
joint_signs=(1, -1, 1, 1, 1, -1, 1),
|
||||
gripper_config=(8, 195, 152),
|
||||
),
|
||||
joint_signs=(1, -1, 1, 1, 1, -1, 1),
|
||||
gripper_config=(8, 195, 152),
|
||||
),
|
||||
# Left UR
|
||||
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBEIA-if00-port0": DynamixelRobotConfig(
|
||||
joint_ids=(1, 2, 3, 4, 5, 6),
|
||||
joint_offsets=(
|
||||
0,
|
||||
1 * np.pi / 2 + np.pi,
|
||||
np.pi / 2 + 0 * np.pi,
|
||||
0 * np.pi + np.pi / 2,
|
||||
np.pi - 2 * np.pi / 2,
|
||||
-1 * np.pi / 2 + 2 * np.pi,
|
||||
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBEIA-if00-port0":
|
||||
DynamixelRobotConfig(
|
||||
joint_ids=(1, 2, 3, 4, 5, 6),
|
||||
joint_offsets=(
|
||||
0,
|
||||
1 * np.pi / 2 + np.pi,
|
||||
np.pi / 2 + 0 * np.pi,
|
||||
0 * np.pi + np.pi / 2,
|
||||
np.pi - 2 * np.pi / 2,
|
||||
-1 * np.pi / 2 + 2 * np.pi,
|
||||
),
|
||||
joint_signs=(1, 1, -1, 1, 1, 1),
|
||||
gripper_config=(7, 20, -22),
|
||||
),
|
||||
joint_signs=(1, 1, -1, 1, 1, 1),
|
||||
gripper_config=(7, 20, -22),
|
||||
),
|
||||
# Right UR
|
||||
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBG6A-if00-port0": DynamixelRobotConfig(
|
||||
joint_ids=(1, 2, 3, 4, 5, 6),
|
||||
joint_offsets=(
|
||||
np.pi + 0 * np.pi,
|
||||
2 * np.pi + np.pi / 2,
|
||||
2 * np.pi + np.pi / 2,
|
||||
2 * np.pi + np.pi / 2,
|
||||
1 * np.pi,
|
||||
3 * np.pi / 2,
|
||||
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT7WBG6A-if00-port0":
|
||||
DynamixelRobotConfig(
|
||||
joint_ids=(1, 2, 3, 4, 5, 6),
|
||||
joint_offsets=(
|
||||
np.pi + 0 * np.pi,
|
||||
2 * np.pi + np.pi / 2,
|
||||
2 * np.pi + np.pi / 2,
|
||||
2 * np.pi + np.pi / 2,
|
||||
1 * np.pi,
|
||||
3 * np.pi / 2,
|
||||
),
|
||||
joint_signs=(1, 1, -1, 1, 1, 1),
|
||||
gripper_config=(7, 286, 248),
|
||||
),
|
||||
joint_signs=(1, 1, -1, 1, 1, 1),
|
||||
gripper_config=(7, 286, 248),
|
||||
),
|
||||
}
|
||||
|
||||
|
||||
class GelloAgent(Agent):
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
port: str,
|
||||
|
|
@ -114,18 +119,18 @@ class GelloAgent(Agent):
|
|||
start_joints: Optional[np.ndarray] = None,
|
||||
):
|
||||
if dynamixel_config is not None:
|
||||
self._robot = dynamixel_config.make_robot(
|
||||
port=port, start_joints=start_joints
|
||||
)
|
||||
self._robot = dynamixel_config.make_robot(port=port,
|
||||
start_joints=start_joints)
|
||||
else:
|
||||
assert os.path.exists(port), port
|
||||
assert port in PORT_CONFIG_MAP, f"Port {port} not in config map"
|
||||
|
||||
config = PORT_CONFIG_MAP[port]
|
||||
self._robot = config.make_robot(port=port, start_joints=start_joints)
|
||||
self._robot = config.make_robot(port=port,
|
||||
start_joints=start_joints)
|
||||
|
||||
def act(self, obs: Dict[str, np.ndarray]) -> np.ndarray:
|
||||
return self._robot.get_joint_state()
|
||||
# return self._robot.get_joint_state()
|
||||
dyna_joints = self._robot.get_joint_state()
|
||||
# current_q = dyna_joints[:-1] # last one dim is the gripper
|
||||
current_gripper = dyna_joints[-1] # last one dim is the gripper
|
||||
|
|
@ -137,3 +142,8 @@ class GelloAgent(Agent):
|
|||
else:
|
||||
self._robot.set_torque_mode(False)
|
||||
return dyna_joints
|
||||
|
||||
def get_gello_joint_state(self) -> Tuple[np.ndarray, float]:
|
||||
dyna_joints = self._robot.get_joint_state()
|
||||
gripper = dyna_joints[-1]
|
||||
return dyna_joints[:-1], gripper
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue