UPDATE emancro gello

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

@@ -45,20 +45,20 @@ 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=(
+            3 * np.pi / 2,
+            1 * np.pi / 2,
+            3 * np.pi / 2,
+            1 * np.pi / 2,
+            3 * 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(
@@ -125,7 +125,7 @@ def __init__(
             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

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