UPDATE emancro gello

BUILD.bazel

@@ -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"],
+)

experiments/run_env.py

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

gello/agents/gello_agent.py

@@ -29,9 +29,9 @@ def __post_init__(self):
  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,87 +45,92 @@ def make_robot(
 
 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,
  dynamixel_config: Optional[DynamixelRobotConfig] = None,
  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 @@ def act(self, obs: Dict[str, np.ndarray]) -> np.ndarray:
  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

gello/dynamixel/driver.py

@@ -145,6 +145,33 @@ def __init__(
  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,23 +275,42 @@ def close(self):
 
 def main():
  # Set the port, baudrate, and servo IDs
- ids = [1]
+ ids = [1,2,3,4,5,6,7]
 
  # Create a DynamixelDriver instance
  try:
  driver = DynamixelDriver(ids)
  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()

gello/robots/dynamixel.py

@@ -30,6 +30,7 @@ def __init__(
  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 @@ def __init__(
  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 @@ def get_joint_state(self) -> np.ndarray:
 
  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 @@ def get_joint_state(self) -> np.ndarray:
  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):

installdeps.sh

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