diff --git a/gello.py b/gello.py index 406a211..3f1f0aa 100644 --- a/gello.py +++ b/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( diff --git a/gello_get_offset.py b/gello_get_offset.py new file mode 100644 index 0000000..d09d540 --- /dev/null +++ b/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)) \ No newline at end of file diff --git a/main.py b/main.py index dd0e03b..ab9387a 100644 --- a/main.py +++ b/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"]) diff --git a/utils.py b/utils.py index cad8c0a..3a9fcb8 100644 --- a/utils.py +++ b/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): """