working trained policy

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

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
 

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:
+        #with open(config_file, "r") as file:
-            config = yaml.safe_load(file)
+        #    config = yaml.safe_load(file)
-        for param, value in config.get("robot", {}).get("arm_bus", {}).items():
+        #for param, value in config.get("robot", {}).get("arm_bus", {}).items():
-            self.arm_bus.write(param, value)
+        #    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"))
+        #move arm elbow to half way
-        print(self.arm_bus.read("Present_Position",["shoulder_yaw","shoulder_roll","elbow_flex","wrist_roll","wrist_yaw","wrist_pitch"]))
+        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
+        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

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)