diff --git a/teleop/open_television/television.py b/teleop/open_television/television.py index c0be8fc..9087f81 100644 --- a/teleop/open_television/television.py +++ b/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 diff --git a/teleop/open_television/tv_wrapper.py b/teleop/open_television/tv_wrapper.py index e4abfac..e399a84 100644 --- a/teleop/open_television/tv_wrapper.py +++ b/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 """ diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index fa040f7..1e64c7b 100644 --- a/teleop/robot_control/robot_arm.py +++ b/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) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index b6bb76c..dc8bc8c 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/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)