MVP gello teleop

gello.py

@@ -9,8 +9,8 @@ import time
 from main import config, HOST, PORT
 import threading
 
-joystick_max = -0.9583490120438358
-joystick_min = -1.8495918498053934
+joystick_max = 0.89
+joystick_min = 0
 gripper_max = 0.93
 gripper_min = 0.0
 
@@ -40,7 +40,7 @@ class RobotControllerNode(Node):
         self.gripper_joint_name = "left_outer_knuckle_joint"
         self.sequence_step = 0
 
-        self.timer = self.create_timer(1.0, self.run_sequence)
+        self.timer = self.create_timer(0.1, self.run_sequence)
 
     def js_callback(self, msg: JointState):
         self.last_js = msg
@@ -114,6 +114,7 @@ class RobotControllerNode(Node):
 
     def run_sequence(self):
         print(f"STEP #{self.sequence_step}")
+        self.robot_client.move_joint(list(self.last_gello.position[:-1]))
         if not self.waiting_for_gripper:
             self.waiting_for_gripper = True
             threading.Thread(

gello_get_offset.py

@@ -0,0 +1,98 @@
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Tuple
+
+import numpy as np
+import tyro 
+
+from franka_gello_state_publisher.driver import DynamixelDriver
+
+MENAGERIE_ROOT: Path = Path(__file__).parent / "third_party" / "mujoco_menagerie"
+
+
+@dataclass
+class Args:
+    port: str = "/dev/ttyUSB0"
+    """The port that GELLO is connected to."""
+
+    start_joints: Tuple[float, ...] = (0, 0, 0, 0, 0, 0)
+    """The joint angles that the GELLO is placed in at (in radians)."""
+
+    joint_signs: Tuple[float, ...] = (1, 1, -1, 1, 1, 1)
+    """The joint angles that the GELLO is placed in at (in radians)."""
+
+    gripper: bool = True
+    """Whether or not the gripper is attached."""
+
+    def __post_init__(self):
+        assert len(self.joint_signs) == len(self.start_joints)
+        for idx, j in enumerate(self.joint_signs):
+            assert (
+                j == -1 or j == 1
+            ), f"Joint idx: {idx} should be -1 or 1, but got {j}."
+
+    @property
+    def num_robot_joints(self) -> int:
+        return len(self.start_joints)
+
+    @property
+    def num_joints(self) -> int:
+        extra_joints = 1 if self.gripper else 0
+        return self.num_robot_joints + extra_joints
+
+
+def get_config(args: Args) -> None:
+    joint_ids = list(range(1, args.num_joints + 1))
+    driver = DynamixelDriver(joint_ids, port=args.port, baudrate=2000000)
+
+    # assume that the joint state shouold be args.start_joints
+    # find the offset, which is a multiple of np.pi/2 that minimizes the error between the current joint state and args.start_joints
+    # this is done by brute force, we seach in a range of +/- 8pi
+
+    def get_error(offset: float, index: int, joint_state: np.ndarray) -> float:
+        joint_sign_i = args.joint_signs[index]
+        joint_i = joint_sign_i * (joint_state[index] - offset)
+        start_i = args.start_joints[index]
+        return np.abs(joint_i - start_i)
+
+    for _ in range(10):
+        driver.get_joints()  # warmup
+
+    for _ in range(1):
+        best_offsets = []
+        curr_joints = driver.get_joints()
+        for i in range(args.num_robot_joints):
+            best_offset = 0
+            best_error = 1e6
+            for offset in np.linspace(
+                -8 * np.pi, 8 * np.pi, 8 * 4 + 1
+            ):  # intervals of pi/2
+                error = get_error(offset, i, curr_joints)
+                if error < best_error:
+                    best_error = error
+                    best_offset = offset
+            best_offsets.append(best_offset)
+        print()
+        print("best offsets               : ", [f"{x:.3f}" for x in best_offsets])
+        print(
+            "best offsets function of pi: ["
+            + ", ".join([f"{int(np.round(x/(np.pi/2)))}*np.pi/2" for x in best_offsets])
+            + " ]",
+        )
+        if args.gripper:
+            print(
+                "gripper open (degrees)       ",
+                np.rad2deg(driver.get_joints()[-1]) - 0.2,
+            )
+            print(
+                "gripper close (degrees)      ",
+                np.rad2deg(driver.get_joints()[-1]) - 42,
+            )
+
+
+def main(args: Args) -> None:
+    get_config(args)
+
+
+if __name__ == "__main__":
+    main(tyro.cli(Args))

main.py

@@ -35,22 +35,25 @@ config = {
     "final_moveaway": {
         'pos': [0.06232913033349278, 0.28220623060555067, 0.1939445619236099], 'ori': [0.6689183667946061, -0.6855402199272477, 0.10599550652863488, -0.267110048524341]
     },
-    "joint_speed_test": {
-        'pos': [-0.014667855657250158, 0.21178473592030767, 0.6099448870104303], 'ori': [0.8973159098098845, -0.2554666640875886, 0.044440682660961045, -0.35719177939399194]
+    "zero": {
+        'joint': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
     },
+    "gello": {
+        'joint': [ -1.61, -0.25, 1.7, 2.08, 1.53, -0.06]
+    }
 }
 
 def main():
     robot = RobotClient(HOST, PORT)
-    robot.set_joint_maxacc(1)
-    robot.set_joint_maxvelc(1)
-    robot.set_end_max_line_velc(1)
-    robot.set_end_max_line_acc(1)
-    robot.set_end_max_angle_acc(1)
-    robot.set_end_max_angle_velc(1)
+    robot.set_joint_maxacc(0.5)
+    robot.set_joint_maxvelc(0.5)
+    robot.set_end_max_line_velc(0.5)
+    robot.set_end_max_line_acc(0.5)
+    robot.set_end_max_angle_acc(0.5)
+    robot.set_end_max_angle_velc(0.5)
 
-    robot.move_to_pose_joint(config["final_moveaway"])
-    robot.move_to_pose_cart(config["pre_grasp_position"])
+    robot.move_joint(config["gello"]["joint"])
+    # robot.move_to_pose_cart(config["pre_grasp_position"])
     # robot.move_to_pose_cart(config["grasp_position"])
     # robot.move_to_pose_cart(config["post_grasp_position"])
     # robot.move_to_pose_cart(config["pre_insert_position"])

utils.py

@@ -176,12 +176,13 @@ class RobotClient:
             raise RuntimeError(f"move_line failed: {ret}")
         self.wait_untill_motion_is_done(10.0)
 
-    def move_joint(self, joint_radian: list)  -> None:
+    def move_joint(self, joint_radian: list)  -> bool:
         cmd = {"command": "move_joint", "joint_radian": joint_radian}
         ret = self._send_single_json(cmd, 10.0)
         if ret.get("ret") != 0:
             raise RuntimeError(f"move_joint failed: {ret}")
         self.wait_untill_motion_is_done(10.0)
+        return True
 
     def wait_untill_motion_is_done(self, timeout=10.0):
         """