robossembler/gello_software
7
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

UPDATE emancro gello

Browse source
This commit is contained in:
Yuwei Xia 2024-09-23 13:19:29 -07:00
parent daae81f4a7
commit 8e84ad0222
6 changed files with 153 additions and 64 deletions
Download patch file Download diff file Expand all files Collapse all files

17
BUILD.bazel Normal file
View file

@ -0,0 +1,17 @@
load("@rules_python//python:defs.bzl", "py_library", "py_binary")
load("@python_deps//:requirements.bzl", "requirement")
py_library(
name = "gello",
srcs = glob(["*.py", "**/*.py"]),
deps = [
requirement("numpy"),
requirement("torchvision"),
requirement("torch"),
requirement("ultralytics"),
requirement("opencv-python"),
requirement("pyquaternion"),
],
visibility = ["//visibility:public"],
)

9
experiments/run_env.py
View file

@ -117,7 +117,7 @@ def main(args):
)
if args.start_joints is None:
reset_joints = np.deg2rad(
[0, -90, 90, -90, -90, 0, 0]
[90, 0, 0, 0, 0, 0, 0, 0]
) # Change this to your own reset joints
else:
reset_joints = args.start_joints
@ -146,10 +146,12 @@ def main(args):
raise ValueError("Invalid agent name")
# going to start position
print("Going to start position")
print(f"Going to start position {env.get_obs()}")
start_pos = agent.act(env.get_obs())
print(f"start_pos as: {start_pos}" )
obs = env.get_obs()
joints = obs["joint_positions"]
print(f"joints {joints}")
abs_deltas = np.abs(start_pos - joints)
id_max_joint_delta = np.argmax(abs_deltas)
@ -189,7 +191,7 @@ def main(args):
obs = env.get_obs()
joints = obs["joint_positions"]
action = agent.act(obs)
if (action - joints > 0.5).any():
if (action - joints > 0.5*10).any():
print("Action is too big")
# print which joints are too big
@ -220,6 +222,7 @@ def main(args):
flush=True,
)
action = agent.act(obs)
print(f"action: {action}")
dt = datetime.datetime.now()
if args.use_save_interface:
state = kb_interface.update()

128
gello/agents/gello_agent.py
View file

@ -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

50
gello/dynamixel/driver.py
View file

@ -145,6 +145,33 @@ class DynamixelDriver(DynamixelDriverProtocol):
self._stop_thread = Event()
self._start_reading_thread()
def move_single_joint(self, dxl_id, joint_angle: float):
if not self._torque_enabled:
raise RuntimeError("Torque must be enabled to set joint angles")
result, error = self._packetHandler.write4ByteTxRx(
self._portHandler, dxl_id, ADDR_GOAL_POSITION, int(joint_angle * 2048 / np.pi)
)
if result != COMM_SUCCESS:
print(f"move_single_joint: {dxl_id}, {joint_angle}, {result}, {error}")
# raise RuntimeError(f"Failed to write goal position for Dynamixel with ID {dxl_id}")
def move_joints(self, joint_angles: Sequence[float]):
if len(joint_angles) != len(self._ids):
raise ValueError(
"The length of joint_angles must match the number of servos"
)
if not self._torque_enabled:
raise RuntimeError("Torque must be enabled to set joint angles")
# Write goal position
for dxl_id, angle in zip(self._ids, joint_angles):
self.move_single_joint(dxl_id, angle)
def set_joints(self, joint_angles: Sequence[float]):
if len(joint_angles) != len(self._ids):
raise ValueError(
@ -248,7 +275,7 @@ class DynamixelDriver(DynamixelDriverProtocol):
def main():
# Set the port, baudrate, and servo IDs
ids = [1]
ids = [1,2,3,4,5,6,7]
# Create a DynamixelDriver instance
try:
@ -256,15 +283,34 @@ def main():
except FileNotFoundError:
driver = DynamixelDriver(ids, port="/dev/cu.usbserial-FT7WBMUB")
joint_angles = driver.get_joints()
print(f"Joint angles for IDs {ids}: {joint_angles}")
# Test setting torque mode
driver.set_torque_mode(True)
driver.set_torque_mode(False)
# driver.set_torque_mode(False)
# driver.move_single_joint(1, 4)
# driver.move_single_joint(2, 0.2)
# driver.move_single_joint(3, 2)
# driver.move_single_joint(4, 3)
# driver.move_single_joint(5, 1.5)
# driver.move_single_joint(6, 2.5)
# driver.move_single_joint(7, 0.25)
init_angles = np.array([4,0,2,2.5,1.5,3,3.5])
driver.move_joints(init_angles)
# Test reading the joint angles
try:
while True:
joint_angles = driver.get_joints()
print(f"Joint angles for IDs {ids}: {joint_angles}")
next_joint = joint_angles - 0.05
time.sleep(1.0)
print(f"next_joint: {next_joint}")
driver.move_joints(joint_angles)
time.sleep(5.0)
# print(f"Joint angles for IDs {ids[1]}: {joint_angles[1]}")
except KeyboardInterrupt:
driver.close()

6
gello/robots/dynamixel.py
View file

@ -30,6 +30,7 @@ class DynamixelRobot(Robot):
if gripper_config is not None:
assert joint_offsets is not None
assert joint_signs is not None
print("add gripper...")
# joint_ids.append(gripper_config[0])
# joint_offsets.append(0.0)
@ -41,6 +42,7 @@ class DynamixelRobot(Robot):
gripper_config[1] * np.pi / 180,
gripper_config[2] * np.pi / 180,
)
print(f"joint_ids: {joint_ids}")
else:
self.gripper_open_close = None
@ -109,10 +111,13 @@ class DynamixelRobot(Robot):
if self.gripper_open_close is not None:
# map pos to [0, 1]
# print(f"pos: {pos[-1]}")
# print(f"gripper_open_close: {self.gripper_open_close}")
g_pos = (pos[-1] - self.gripper_open_close[0]) / (
self.gripper_open_close[1] - self.gripper_open_close[0]
)
g_pos = min(max(0, g_pos), 1)
# print(f"g_pos: {g_pos}")
pos[-1] = g_pos
if self._last_pos is None:
@ -125,6 +130,7 @@ class DynamixelRobot(Robot):
return pos
def command_joint_state(self, joint_state: np.ndarray) -> None:
print(f"Command driver here as {(joint_state + self._joint_offsets).tolist()}")
self._driver.set_joints((joint_state + self._joint_offsets).tolist())
def set_torque_mode(self, mode: bool):

7
installdeps.sh Executable file
View file

@ -0,0 +1,7 @@
#!/bin/bash
git submodule init
git submodule update
pip install -r requirements.txt
pip install -e .
pip install -e third_party/DynamixelSDK/python