From a72db65f62c3fdcaa4fd1cd57c7a556156f06a27 Mon Sep 17 00:00:00 2001 From: nepyope Date: Tue, 15 Apr 2025 15:41:24 +0200 Subject: [PATCH] working trained policy --- lerobot/common/policies/factory.py | 2 +- lerobot/common/robot_devices/control_utils.py | 5 +- lerobot/common/robot_devices/robots/hopejr.py | 129 ++++++++++++++++-- lerobot/scripts/control_robot.py | 4 +- 4 files changed, 124 insertions(+), 16 deletions(-) diff --git a/lerobot/common/policies/factory.py b/lerobot/common/policies/factory.py index 5cb2fd52..932e65aa 100644 --- a/lerobot/common/policies/factory.py +++ b/lerobot/common/policies/factory.py @@ -106,6 +106,6 @@ def make_policy( policy = policy_cls(policy_cfg) policy.load_state_dict(policy_cls.from_pretrained(pretrained_policy_name_or_path).state_dict()) - policy.to(get_safe_torch_device(hydra_cfg.device)) + #policy.to(get_safe_torch_device(hydra_cfg.device)) return policy diff --git a/lerobot/common/robot_devices/control_utils.py b/lerobot/common/robot_devices/control_utils.py index 345e58f5..cf7320f1 100644 --- a/lerobot/common/robot_devices/control_utils.py +++ b/lerobot/common/robot_devices/control_utils.py @@ -94,7 +94,7 @@ def predict_action(observation, policy, device, use_amp): observation = copy(observation) with ( torch.inference_mode(), - torch.autocast(device_type=device.type) if device.type == "cuda" and use_amp else nullcontext(), + torch.autocast(device_type=device), ): # Convert to pytorch format: channel first and float32 in [0,1] with batch dimension for name in observation: @@ -165,7 +165,8 @@ def init_policy(pretrained_policy_name_or_path, policy_overrides): policy = make_policy(hydra_cfg=hydra_cfg, pretrained_policy_name_or_path=pretrained_policy_path) # Check device is available - device = get_safe_torch_device(hydra_cfg.device, log=True) + #device = get_safe_torch_device(hydra_cfg.device, log=True) + device="mps" use_amp = hydra_cfg.use_amp policy_fps = hydra_cfg.env.fps diff --git a/lerobot/common/robot_devices/robots/hopejr.py b/lerobot/common/robot_devices/robots/hopejr.py index 9fce20d8..93808c62 100644 --- a/lerobot/common/robot_devices/robots/hopejr.py +++ b/lerobot/common/robot_devices/robots/hopejr.py @@ -737,7 +737,7 @@ class Tac_Man: class HopeJrRobot: robot_type = "hopejr" def __init__(self, cameras=None): - self.arm_port = "/dev/tty.usbserial-140" + self.arm_port = "/dev/tty.usbmodem585A0082361" self.hand_port = "/dev/tty.usbmodem58760436961" self.cameras = {} config = OpenCVCameraConfig(fps=60, width=1920, height=1080) @@ -805,11 +805,11 @@ class HopeJrRobot: self.glove = None def apply_arm_config(self, config_file): - with open(config_file, "r") as file: - config = yaml.safe_load(file) - for param, value in config.get("robot", {}).get("arm_bus", {}).items(): - self.arm_bus.write(param, value) - + #with open(config_file, "r") as file: + # config = yaml.safe_load(file) + #for param, value in config.get("robot", {}).get("arm_bus", {}).items(): + # self.arm_bus.write(param, value) + return def apply_hand_config(config_file, robot): with open(config_file, "r") as file: config = yaml.safe_load(file) @@ -924,12 +924,16 @@ class HopeJrRobot: self.hand_bus.connect() #read pos print(self.hand_bus.read("Present_Position")) - print(self.arm_bus.read("Present_Position", "shoulder_pitch")) - print(self.arm_bus.read("Present_Position",["shoulder_yaw","shoulder_roll","elbow_flex","wrist_roll","wrist_yaw","wrist_pitch"])) + #move arm elbow to half way + self.arm_bus.write("Goal_Position", 2000, "elbow_flex") + self.arm_bus.write("Goal_Position", 700, "wrist_roll") + self.arm_bus.write("Goal_Position", 1150, "shoulder_roll") + #print(self.arm_bus.read("Present_Position", "shoulder_pitch")) + #print(self.arm_bus.read("Present_Position",["shoulder_yaw","shoulder_roll","elbow_flex","wrist_roll","wrist_yaw","wrist_pitch"])) #robot.arm_bus.write("Goal_Position", robot.arm_calib_dict["start_pos"][0]*1 +robot.arm_calib_dict["end_pos"][0]*0, ["wrist_roll"]) for i in range(10): time.sleep(0.1) - self.apply_arm_config('examples/hopejr/settings/config.yaml') + #self.apply_arm_config('examples/hopejr/settings/config.yaml') # #calibrate arm arm_calibration = self.get_arm_calibration() @@ -948,7 +952,7 @@ class HopeJrRobot: #calibrate hand hand_calibration = self.get_hand_calibration() self.glove = HomonculusGlove(serial_port = "/dev/tty.usbmodem1101") - calibrate_glove = True + calibrate_glove = False if calibrate_glove: self.glove.run_calibration() @@ -959,7 +963,7 @@ class HopeJrRobot: self.glove.set_calibration(calib_dict) self.hand_bus.set_calibration(hand_calibration) - self.arm_bus.set_calibration(arm_calibration) + #self.arm_bus.set_calibration(arm_calibration) for camera in self.cameras.values(): camera.connect() @@ -1088,4 +1092,105 @@ class HopeJrRobot: self.is_connected = False def capture_observation(self): - return \ No newline at end of file + self.apply_arm_config('examples/hopejr/settings/config.yaml') + #robot.arm_bus.write("Acceleration", 50, "shoulder_yaw") + joint_names = ["shoulder_pitch", "shoulder_yaw", "shoulder_roll", "elbow_flex", "wrist_roll", "wrist_yaw", "wrist_pitch"] + #only wrist roll + #joint_names = ["shoulder_pitch"] + joint_values = self.exoskeleton.read(motor_names=joint_names) + + #joint_values = joint_values.round().astype(int) + joint_dict = {k: v for k, v in zip(joint_names, joint_values, strict=False)} + + motor_values = [] + motor_names = [] + motor_names += ["shoulder_pitch", "shoulder_yaw", "shoulder_roll", "elbow_flex", "wrist_roll", "wrist_yaw", "wrist_pitch"] + #motor_names += ["shoulder_pitch"] + motor_values += [joint_dict[name] for name in motor_names] + #remove 50 from shoulder_roll + #motor_values += [joint_dict[name] for name in motor_names] + + motor_values = np.array(motor_values) + motor_values = np.clip(motor_values, 0, 100) + + print(motor_names, motor_values) + freeze_fingers = False + if not freeze_fingers:#include hand + hand_joint_names = [] + hand_joint_names += ["thumb_3", "thumb_2", "thumb_1", "thumb_0"]#, "thumb_3"] + hand_joint_names += ["index_0", "index_1", "index_2"] + hand_joint_names += ["middle_0", "middle_1", "middle_2"] + hand_joint_names += ["ring_0", "ring_1", "ring_2"] + hand_joint_names += ["pinky_0", "pinky_1", "pinky_2"] + hand_joint_values = self.glove.read(hand_joint_names) + hand_joint_values = hand_joint_values.round( ).astype(int) + hand_joint_dict = {k: v for k, v in zip(hand_joint_names, hand_joint_values, strict=False)} + + hand_motor_values = [] + hand_motor_names = [] + + # Thumb + hand_motor_names += ["thumb_basel_rotation", "thumb_mcp", "thumb_pip", "thumb_dip"]#, "thumb_MCP"] + hand_motor_values += [ + hand_joint_dict["thumb_3"], + hand_joint_dict["thumb_2"], + hand_joint_dict["thumb_1"], + hand_joint_dict["thumb_0"] + ] + + # # Index finger + index_splay = 0.1 + hand_motor_names += ["index_flexor", "index_pinky_side", "index_thumb_side"] + hand_motor_values += [ + hand_joint_dict["index_2"], + (100 - hand_joint_dict["index_0"]) * index_splay + hand_joint_dict["index_1"] * (1 - index_splay), + hand_joint_dict["index_0"] * index_splay + hand_joint_dict["index_1"] * (1 - index_splay), + ] + + # Middle finger + middle_splay = 0.1 + hand_motor_names += ["middle_flexor", "middle_pinky_side", "middle_thumb_side"] + hand_motor_values += [ + hand_joint_dict["middle_2"], + hand_joint_dict["middle_0"] * middle_splay + hand_joint_dict["middle_1"] * (1 - middle_splay), + (100 - hand_joint_dict["middle_0"]) * middle_splay + hand_joint_dict["middle_1"] * (1 - middle_splay), + ] + + # # Ring finger + ring_splay = 0.1 + hand_motor_names += ["ring_flexor", "ring_pinky_side", "ring_thumb_side"] + hand_motor_values += [ + hand_joint_dict["ring_2"], + (100 - hand_joint_dict["ring_0"]) * ring_splay + hand_joint_dict["ring_1"] * (1 - ring_splay), + hand_joint_dict["ring_0"] * ring_splay + hand_joint_dict["ring_1"] * (1 - ring_splay), + ] + + # # Pinky finger + pinky_splay = -.1 + hand_motor_names += ["pinky_flexor", "pinky_pinky_side", "pinky_thumb_side"] + hand_motor_values += [ + hand_joint_dict["pinky_2"], + hand_joint_dict["pinky_0"] * pinky_splay + hand_joint_dict["pinky_1"] * (1 - pinky_splay), + (100 - hand_joint_dict["pinky_0"]) * pinky_splay + hand_joint_dict["pinky_1"] * (1 - pinky_splay), + ] + + hand_motor_values = np.array(hand_motor_values) + hand_motor_values = np.clip(hand_motor_values, 0, 100) + + all_pos = np.concatenate((motor_values, hand_motor_values)) + all_pos = torch.tensor(all_pos, dtype=torch.float32) + obs = {"observation.state": all_pos} + + # Capture images from cameras + images = {} + for name in self.cameras: + before_camread_t = time.perf_counter() + images[name] = self.cameras[name].async_read() + images[name] = torch.from_numpy(images[name]) + # self.logs[f"read_camera_{name}_dt_s"] = self.cameras[name].logs["delta_timestamp_s"] + # self.logs[f"async_read_camera_{name}_dt_s"] = time.perf_counter() - before_camread_t + + for name in self.cameras: + obs[f"observation.images.{name}"] = images[name] + + return obs \ No newline at end of file diff --git a/lerobot/scripts/control_robot.py b/lerobot/scripts/control_robot.py index a68cd798..fe469202 100644 --- a/lerobot/scripts/control_robot.py +++ b/lerobot/scripts/control_robot.py @@ -225,7 +225,8 @@ def record( # Load pretrained policy if pretrained_policy_name_or_path is not None: policy, policy_fps, device, use_amp = init_policy(pretrained_policy_name_or_path, policy_overrides) - + device="mps" + policy.to(device) if fps is None: fps = policy_fps logging.warning(f"No fps provided, so using the fps from policy config ({policy_fps}).") @@ -509,6 +510,7 @@ if __name__ == "__main__": del kwargs["robot_path"] del kwargs["robot_overrides"] + robot_cfg = init_hydra_config(robot_path, robot_overrides) robot = make_robot(robot_cfg)