[update] under main operation control, teleoperate and record.

teleop/open_television/television.py

@@ -193,27 +193,9 @@ class TeleVision:
     
     async def main_image_binocular(self, session, fps=60):
         if self.use_hand_tracking:
-            session.upsert(
-                Hands(
-                    stream=True,
-                    key="hands",
-                    showLeft=False,
-                    showRight=False
-                ),
-                # we place this into the background children list, so that it is
-                # not affected by the global rotation
-                to="bgChildren",
-            )
+            session.upsert @ Hands(fps=fps, stream=True, key="hands", showLeft=False, showRight=False)
         else:
-            session.upsert(
-                MotionControllers(
-                    stream=True, 
-                    key="motion-controller",
-                    showLeft=False,
-                    showRight=False,
-                ),
-                to="bgChildren",
-            )
+            session.upsert @ MotionControllers(fps=fps, stream=True, key="motion-controller", showLeft=False, showRight=False)
 
         while True:
             display_image = cv2.cvtColor(self.img_array, cv2.COLOR_BGR2RGB)
@@ -230,7 +212,7 @@ class TeleVision:
                         # Note that these two masks are associated with left eye’s camera and the right eye’s camera.
                         layers=1,
                         format="jpeg",
-                        quality=50,
+                        quality=90,
                         key="background-left",
                         interpolate=True,
                     ),
@@ -241,7 +223,7 @@ class TeleVision:
                         distanceToCamera=1,
                         layers=2,
                         format="jpeg",
-                        quality=50,
+                        quality=90,
                         key="background-right",
                         interpolate=True,
                     ),
@@ -253,25 +235,10 @@ class TeleVision:
 
     async def main_image_monocular(self, session, fps=60):
         if self.use_hand_tracking:
-            session.upsert(
-                Hands(
-                    stream=True,
-                    key="hands",
-                    showLeft=False,
-                    showRight=False
-                ),
-                to="bgChildren",
-            )
+            session.upsert @ Hands(fps=fps, stream=True, key="hands", showLeft=False, showRight=False)
         else:
-            session.upsert(
-                MotionControllers(
-                    stream=True, 
-                    key="motion-controller",
-                    showLeft=False,
-                    showRight=False,
-                ),
-                to="bgChildren",
-            )
+            session.upsert @ MotionControllers(fps=fps, stream=True, key="motion-controller", showLeft=False, showRight=False)
+
         while True:
             display_image = cv2.cvtColor(self.img_array, cv2.COLOR_BGR2RGB)
             # aspect_ratio = self.img_width / self.img_height

teleop/open_television/tv_wrapper.py

@@ -1,5 +1,5 @@
 import numpy as np
-from open_television.television import TeleVision
+from television import TeleVision
 from dataclasses import dataclass, field
 
 """

teleop/robot_control/robot_arm.py

@@ -11,8 +11,10 @@ from unitree_sdk2py.utils.crc import CRC
 from unitree_sdk2py.idl.unitree_go.msg.dds_ import ( LowCmd_  as go_LowCmd, LowState_ as go_LowState)  # idl for h1
 from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_
 
-kTopicLowCommand = "rt/lowcmd"
+# kTopicLowCommand = "rt/lowcmd"
 kTopicLowState = "rt/lowstate"
+kTopicLowCommand = "rt/arm_sdk"
+
 G1_29_Num_Motors = 35
 G1_23_Num_Motors = 35
 H1_2_Num_Motors = 35
@@ -149,6 +151,8 @@ class G1_29_ArmController:
         return cliped_arm_q_target
 
     def _ctrl_motor_state(self):
+        self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = 1.0;
+
         while True:
             start_time = time.time()
 
@@ -209,6 +213,9 @@ class G1_29_ArmController:
         while True:
             current_q = self.get_current_dual_arm_q()
             if np.all(np.abs(current_q) < tolerance):
+                for weight in np.arange(1, 0, -0.01):
+                    self.msg.motor_cmd[G1_29_JointIndex.kNotUsedJoint0].q = weight;
+                    time.sleep(0.02)
                 print("[G1_29_ArmController] both arms have reached the home position.")
                 break
             time.sleep(0.05)

teleop/teleop_hand_and_arm.py

@@ -29,7 +29,7 @@ if __name__ == '__main__':
     parser.add_argument('--no-record', dest = 'record', action = 'store_false', help = 'Do not save data')
     parser.set_defaults(record = False)
 
-    parser.add_argument('--xr_mode', type=str, choices=['hand', 'controller'], default='hand', help='Select XR device tracking source')
+    parser.add_argument('--xr-mode', type=str, choices=['hand', 'controller'], default='hand', help='Select XR device tracking source')
     parser.add_argument('--arm', type=str, choices=['G1_29', 'G1_23', 'H1_2', 'H1'], default='G1_29', help='Select arm controller')
     parser.add_argument('--ee', type=str, choices=['dex3', 'gripper', 'inspire1'], help='Select end effector controller')
 
@@ -118,6 +118,13 @@ if __name__ == '__main__':
         hand_ctrl = Inspire_Controller(left_hand_pos_array, right_hand_pos_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array)
     else:
         pass
+
+    # xr mode
+    if args.xr_mode == 'controller':
+        from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient
+        sport_client = LocoClient()  
+        sport_client.SetTimeout(0.0001)
+        sport_client.Init()
     
     if args.record:
         recorder = EpisodeWriter(task_dir = args.task_dir, frequency = args.frequency, rerun_log = True)
@@ -142,6 +149,15 @@ if __name__ == '__main__':
                         left_gripper_value.value = tele_data.left_trigger_value
                     with right_gripper_value.get_lock():
                         right_gripper_value.value = tele_data.right_trigger_value
+                    # quit or damp
+                    if tele_data.tele_state.right_aButton:
+                        running = False
+                    if tele_data.tele_state.right_thumbstick_state:
+                        sport_client.Damp()
+                    # control
+                    sport_client.Move(-tele_data.tele_state.left_thumbstick_value[1]  * 0.3,
+                                      -tele_data.tele_state.left_thumbstick_value[0]  * 0.3,
+                                      -tele_data.tele_state.right_thumbstick_value[0] * 0.3)
                 elif args.ee == 'gripper' and args.xr_mode == 'hand':
                     with left_gripper_value.get_lock():
                         left_gripper_value.value = tele_data.left_pinch_value
@@ -177,19 +193,29 @@ if __name__ == '__main__':
                 # record data
                 if args.record:
                     # dex hand or gripper
-                    if args.ee == "dex3":
+                    if args.ee == "dex3" and args.xr_mode == 'hand':
                         with dual_hand_data_lock:
                             left_hand_state = dual_hand_state_array[:7]
                             right_hand_state = dual_hand_state_array[-7:]
                             left_hand_action = dual_hand_action_array[:7]
                             right_hand_action = dual_hand_action_array[-7:]
-                    elif args.ee == "gripper":
+                    elif args.ee == "gripper" and args.xr_mode == 'hand':
+                        with dual_gripper_data_lock:
+                            left_hand_state = [dual_gripper_state_array[1]]
+                            right_hand_state = [dual_gripper_state_array[0]]
+                            left_hand_action = [dual_gripper_action_array[1]]
+                            right_hand_action = [dual_gripper_action_array[0]]
+                    elif args.ee == "gripper" and args.xr_mode == 'controller':
                         with dual_gripper_data_lock:
                             left_hand_state = [dual_gripper_state_array[1]]
                             right_hand_state = [dual_gripper_state_array[0]]
                             left_hand_action = [dual_gripper_action_array[1]]
                             right_hand_action = [dual_gripper_action_array[0]]
-                    elif args.ee == "inspire1":
+                            current_body_state = arm_ctrl.get_current_motor_q().tolist()
+                            current_body_action = [-tele_data.tele_state.left_thumbstick_value[1]  * 0.3,
+                                                   -tele_data.tele_state.left_thumbstick_value[0]  * 0.3,
+                                                   -tele_data.tele_state.right_thumbstick_value[0] * 0.3]
+                    elif args.ee == "inspire1" and args.xr_mode == 'hand':
                         with dual_hand_data_lock:
                             left_hand_state = dual_hand_state_array[:6]
                             right_hand_state = dual_hand_state_array[-6:]
@@ -200,6 +226,8 @@ if __name__ == '__main__':
                         right_hand_state = []
                         left_hand_action = []
                         right_hand_action = []
+                        current_body_state = []
+                        current_body_action = []
                     # head image
                     current_tv_image = tv_img_array.copy()
                     # wrist image
@@ -245,7 +273,9 @@ if __name__ == '__main__':
                                 "qvel":   [],                           
                                 "torque": [],  
                             }, 
-                            "body": None, 
+                            "body": {
+                                "qpos": current_body_state,
+                            }, 
                         }
                         actions = {
                             "left_arm": {                                   
@@ -268,7 +298,9 @@ if __name__ == '__main__':
                                 "qvel":   [],       
                                 "torque": [], 
                             }, 
-                            "body": None, 
+                            "body": {
+                                "qpos": current_body_action,
+                            }, 
                         }
                         recorder.add_item(colors=colors, depths=depths, states=states, actions=actions)