From 80f48fc7f119081e83c7bd68903d548ce48b016d Mon Sep 17 00:00:00 2001 From: silencht Date: Fri, 31 Oct 2025 10:07:49 +0800 Subject: [PATCH] [bump] teleimager version, fix minor bug, optim ik opts. --- teleop/robot_control/robot_arm.py | 24 +++--- teleop/robot_control/robot_arm_ik.py | 92 +++++++++++++++------- teleop/robot_control/robot_hand_brainco.py | 2 +- teleop/robot_control/robot_hand_inspire.py | 2 +- teleop/robot_control/robot_hand_unitree.py | 6 +- teleop/teleimager | 2 +- teleop/teleop_hand_and_arm.py | 72 +++++++++++------ teleop/utils/episode_writer.py | 2 +- teleop/utils/motion_switcher.py | 4 +- 9 files changed, 131 insertions(+), 75 deletions(-) diff --git a/teleop/robot_control/robot_arm.py b/teleop/robot_control/robot_arm.py index acaa8e2..79d6c4c 100644 --- a/teleop/robot_control/robot_arm.py +++ b/teleop/robot_control/robot_arm.py @@ -114,7 +114,7 @@ class G1_29_ArmController: self.all_motor_q = self.get_current_motor_q() logger_mp.debug(f"Current all body motor state q:\n{self.all_motor_q} \n") logger_mp.debug(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") - logger_mp.info("Lock all joints except two arms...\n") + logger_mp.info("Lock all joints except two arms...") arm_indices = set(member.value for member in G1_29_JointArmIndex) for id in G1_29_JointIndex: @@ -134,7 +134,7 @@ class G1_29_ArmController: self.msg.motor_cmd[id].kp = self.kp_high self.msg.motor_cmd[id].kd = self.kd_high self.msg.motor_cmd[id].q = self.all_motor_q[id] - logger_mp.info("Lock OK!\n") + logger_mp.info("Lock OK!") # initialize publish thread self.publish_thread = threading.Thread(target=self._ctrl_motor_state) @@ -142,7 +142,7 @@ class G1_29_ArmController: self.publish_thread.daemon = True self.publish_thread.start() - logger_mp.info("Initialize G1_29_ArmController OK!\n") + logger_mp.info("Initialize G1_29_ArmController OK!") def _subscribe_motor_state(self): while True: @@ -402,7 +402,7 @@ class G1_23_ArmController: self.all_motor_q = self.get_current_motor_q() logger_mp.info(f"Current all body motor state q:\n{self.all_motor_q} \n") logger_mp.info(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") - logger_mp.info("Lock all joints except two arms...\n") + logger_mp.info("Lock all joints except two arms...") arm_indices = set(member.value for member in G1_23_JointArmIndex) for id in G1_23_JointIndex: @@ -422,7 +422,7 @@ class G1_23_ArmController: self.msg.motor_cmd[id].kp = self.kp_high self.msg.motor_cmd[id].kd = self.kd_high self.msg.motor_cmd[id].q = self.all_motor_q[id] - logger_mp.info("Lock OK!\n") + logger_mp.info("Lock OK!") # initialize publish thread self.publish_thread = threading.Thread(target=self._ctrl_motor_state) @@ -430,7 +430,7 @@ class G1_23_ArmController: self.publish_thread.daemon = True self.publish_thread.start() - logger_mp.info("Initialize G1_23_ArmController OK!\n") + logger_mp.info("Initialize G1_23_ArmController OK!") def _subscribe_motor_state(self): while True: @@ -682,7 +682,7 @@ class H1_2_ArmController: self.all_motor_q = self.get_current_motor_q() logger_mp.info(f"Current all body motor state q:\n{self.all_motor_q} \n") logger_mp.info(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") - logger_mp.info("Lock all joints except two arms...\n") + logger_mp.info("Lock all joints except two arms...") arm_indices = set(member.value for member in H1_2_JointArmIndex) for id in H1_2_JointIndex: @@ -702,7 +702,7 @@ class H1_2_ArmController: self.msg.motor_cmd[id].kp = self.kp_high self.msg.motor_cmd[id].kd = self.kd_high self.msg.motor_cmd[id].q = self.all_motor_q[id] - logger_mp.info("Lock OK!\n") + logger_mp.info("Lock OK!") # initialize publish thread self.publish_thread = threading.Thread(target=self._ctrl_motor_state) @@ -710,7 +710,7 @@ class H1_2_ArmController: self.publish_thread.daemon = True self.publish_thread.start() - logger_mp.info("Initialize H1_2_ArmController OK!\n") + logger_mp.info("Initialize H1_2_ArmController OK!") def _subscribe_motor_state(self): while True: @@ -964,7 +964,7 @@ class H1_ArmController: self.all_motor_q = self.get_current_motor_q() logger_mp.info(f"Current all body motor state q:\n{self.all_motor_q} \n") logger_mp.info(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n") - logger_mp.info("Lock all joints except two arms...\n") + logger_mp.info("Lock all joints except two arms...") for id in H1_JointIndex: if self._Is_weak_motor(id): @@ -976,7 +976,7 @@ class H1_ArmController: self.msg.motor_cmd[id].kd = self.kd_high self.msg.motor_cmd[id].mode = 0x0A self.msg.motor_cmd[id].q = self.all_motor_q[id] - logger_mp.info("Lock OK!\n") + logger_mp.info("Lock OK!") # initialize publish thread self.publish_thread = threading.Thread(target=self._ctrl_motor_state) @@ -984,7 +984,7 @@ class H1_ArmController: self.publish_thread.daemon = True self.publish_thread.start() - logger_mp.info("Initialize H1_ArmController OK!\n") + logger_mp.info("Initialize H1_ArmController OK!") def _subscribe_motor_state(self): while True: diff --git a/teleop/robot_control/robot_arm_ik.py b/teleop/robot_control/robot_arm_ik.py index 0a94d11..7276081 100644 --- a/teleop/robot_control/robot_arm_ik.py +++ b/teleop/robot_control/robot_arm_ik.py @@ -143,13 +143,22 @@ class G1_29_ArmIK: self.opti.minimize(50 * self.translational_cost + self.rotation_cost + 0.02 * self.regularization_cost + 0.1 * self.smooth_cost) opts = { - 'ipopt':{ - 'print_level':0, - 'max_iter':50, - 'tol':1e-6 - }, - 'print_time':False,# print or not - 'calc_lam_p':False # https://github.com/casadi/casadi/wiki/FAQ:-Why-am-I-getting-%22NaN-detected%22in-my-optimization%3F + # CasADi-level options + 'expand': True, + 'detect_simple_bounds': True, + 'calc_lam_p': False, # https://github.com/casadi/casadi/wiki/FAQ:-Why-am-I-getting-%22NaN-detected%22in-my-optimization%3F + 'print_time':False, # print or not + # IPOPT solver options + 'ipopt.sb': 'yes', # disable Ipopt's license message + 'ipopt.print_level': 0, + 'ipopt.max_iter': 30, + 'ipopt.tol': 1e-4, + 'ipopt.acceptable_tol': 5e-4, + 'ipopt.acceptable_iter': 5, + 'ipopt.warm_start_init_point': 'yes', + 'ipopt.derivative_test': 'none', + 'ipopt.jacobian_approximation': 'exact', + # 'ipopt.hessian_approximation': 'limited-memory', } self.opti.solver("ipopt", opts) @@ -369,13 +378,22 @@ class G1_23_ArmIK: self.opti.minimize(50 * self.translational_cost + 0.5 * self.rotation_cost + 0.02 * self.regularization_cost + 0.1 * self.smooth_cost) opts = { - 'ipopt':{ - 'print_level':0, - 'max_iter':50, - 'tol':1e-6 - }, - 'print_time':False,# print or not - 'calc_lam_p':False # https://github.com/casadi/casadi/wiki/FAQ:-Why-am-I-getting-%22NaN-detected%22in-my-optimization%3F + # CasADi-level options + 'expand': True, + 'detect_simple_bounds': True, + 'calc_lam_p': False, # https://github.com/casadi/casadi/wiki/FAQ:-Why-am-I-getting-%22NaN-detected%22in-my-optimization%3F + 'print_time':False, # print or not + # IPOPT solver options + 'ipopt.sb': 'yes', # disable Ipopt's license message + 'ipopt.print_level': 0, + 'ipopt.max_iter': 30, + 'ipopt.tol': 1e-4, + 'ipopt.acceptable_tol': 5e-4, + 'ipopt.acceptable_iter': 5, + 'ipopt.warm_start_init_point': 'yes', + 'ipopt.derivative_test': 'none', + 'ipopt.jacobian_approximation': 'exact', + # 'ipopt.hessian_approximation': 'limited-memory', } self.opti.solver("ipopt", opts) @@ -620,13 +638,22 @@ class H1_2_ArmIK: self.opti.minimize(50 * self.translational_cost + self.rotation_cost + 0.02 * self.regularization_cost + 0.1 * self.smooth_cost) opts = { - 'ipopt':{ - 'print_level':0, - 'max_iter':50, - 'tol':1e-6 - }, - 'print_time':False,# print or not - 'calc_lam_p':False # https://github.com/casadi/casadi/wiki/FAQ:-Why-am-I-getting-%22NaN-detected%22in-my-optimization%3F + # CasADi-level options + 'expand': True, + 'detect_simple_bounds': True, + 'calc_lam_p': False, # https://github.com/casadi/casadi/wiki/FAQ:-Why-am-I-getting-%22NaN-detected%22in-my-optimization%3F + 'print_time':False, # print or not + # IPOPT solver options + 'ipopt.sb': 'yes', # disable Ipopt's license message + 'ipopt.print_level': 0, + 'ipopt.max_iter': 30, + 'ipopt.tol': 1e-4, + 'ipopt.acceptable_tol': 5e-4, + 'ipopt.acceptable_iter': 5, + 'ipopt.warm_start_init_point': 'yes', + 'ipopt.derivative_test': 'none', + 'ipopt.jacobian_approximation': 'exact', + # 'ipopt.hessian_approximation': 'limited-memory', } self.opti.solver("ipopt", opts) @@ -874,13 +901,22 @@ class H1_ArmIK: self.opti.minimize(50 * self.translational_cost + 0.5 * self.rotation_cost + 0.02 * self.regularization_cost + 0.1 * self.smooth_cost) opts = { - 'ipopt':{ - 'print_level':0, - 'max_iter':50, - 'tol':1e-6 - }, - 'print_time':False,# print or not - 'calc_lam_p':False # https://github.com/casadi/casadi/wiki/FAQ:-Why-am-I-getting-%22NaN-detected%22in-my-optimization%3F + # CasADi-level options + 'expand': True, + 'detect_simple_bounds': True, + 'calc_lam_p': False, # https://github.com/casadi/casadi/wiki/FAQ:-Why-am-I-getting-%22NaN-detected%22in-my-optimization%3F + 'print_time':False, # print or not + # IPOPT solver options + 'ipopt.sb': 'yes', # disable Ipopt's license message + 'ipopt.print_level': 0, + 'ipopt.max_iter': 30, + 'ipopt.tol': 1e-4, + 'ipopt.acceptable_tol': 5e-4, + 'ipopt.acceptable_iter': 5, + 'ipopt.warm_start_init_point': 'yes', + 'ipopt.derivative_test': 'none', + 'ipopt.jacobian_approximation': 'exact', + # 'ipopt.hessian_approximation': 'limited-memory', } self.opti.solver("ipopt", opts) diff --git a/teleop/robot_control/robot_hand_brainco.py b/teleop/robot_control/robot_hand_brainco.py index a37b554..9625d5a 100644 --- a/teleop/robot_control/robot_hand_brainco.py +++ b/teleop/robot_control/robot_hand_brainco.py @@ -67,7 +67,7 @@ class Brainco_Controller: hand_control_process.daemon = True hand_control_process.start() - logger_mp.info("Initialize brainco_Controller OK!\n") + logger_mp.info("Initialize brainco_Controller OK!") def _subscribe_hand_state(self): while True: diff --git a/teleop/robot_control/robot_hand_inspire.py b/teleop/robot_control/robot_hand_inspire.py index 3158b99..60ad1b9 100644 --- a/teleop/robot_control/robot_hand_inspire.py +++ b/teleop/robot_control/robot_hand_inspire.py @@ -61,7 +61,7 @@ class Inspire_Controller: hand_control_process.daemon = True hand_control_process.start() - logger_mp.info("Initialize Inspire_Controller OK!\n") + logger_mp.info("Initialize Inspire_Controller OK!") def _subscribe_hand_state(self): while True: diff --git a/teleop/robot_control/robot_hand_unitree.py b/teleop/robot_control/robot_hand_unitree.py index 8cb9e57..93c4ca4 100644 --- a/teleop/robot_control/robot_hand_unitree.py +++ b/teleop/robot_control/robot_hand_unitree.py @@ -13,7 +13,7 @@ import time import os import sys import threading -from multiprocessing import Process, shared_memory, Array, Value, Lock +from multiprocessing import Process, Array, Value, Lock parent2_dir = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) sys.path.append(parent2_dir) @@ -100,7 +100,7 @@ class Dex3_1_Controller: hand_control_process.daemon = True hand_control_process.start() - logger_mp.info("Initialize Dex3_1_Controller OK!\n") + logger_mp.info("Initialize Dex3_1_Controller OK!") def _subscribe_hand_state(self): while True: @@ -304,7 +304,7 @@ class Dex1_1_Gripper_Controller: self.gripper_control_thread.daemon = True self.gripper_control_thread.start() - logger_mp.info("Initialize Dex1_1_Gripper_Controller OK!\n") + logger_mp.info("Initialize Dex1_1_Gripper_Controller OK!") def _subscribe_gripper_state(self): while True: diff --git a/teleop/teleimager b/teleop/teleimager index ab50186..085aa48 160000 --- a/teleop/teleimager +++ b/teleop/teleimager @@ -1 +1 @@ -Subproject commit ab5018691943433c24af4c9a7f3ea0c9a6fbaf3c +Subproject commit 085aa48f9d412467d059498f21889cb70126d93f diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 0eded0c..e4eeb51 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -1,6 +1,5 @@ import time import argparse -import cv2 from multiprocessing import Value, Array, Lock import threading import logging_mp @@ -19,7 +18,7 @@ from teleop.robot_control.robot_arm_ik import G1_29_ArmIK, G1_23_ArmIK, H1_2_Arm from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller, Dex1_1_Gripper_Controller from teleop.robot_control.robot_hand_inspire import Inspire_Controller from teleop.robot_control.robot_hand_brainco import Brainco_Controller -from teleimager import ImageClient +from teleimager.image_client import ImageClient from teleop.utils.episode_writer import EpisodeWriter from teleop.utils.ipc import IPC_Server from teleop.utils.motion_switcher import MotionSwitcher, LocoClientWrapper @@ -76,7 +75,7 @@ def get_state() -> dict: if __name__ == '__main__': parser = argparse.ArgumentParser() # basic control parameters - parser.add_argument('--frequency', type = float, default = 30.0, help = 'control and record \'s frequency') + parser.add_argument('--frequency', type = float, default = 60.0, help = 'control and record \'s frequency') 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=['dex1', 'dex3', 'inspire1', 'brainco'], help='Select end effector controller') @@ -86,7 +85,7 @@ if __name__ == '__main__': parser.add_argument('--sim', action = 'store_true', help = 'Enable isaac simulation mode') parser.add_argument('--ipc', action = 'store_true', help = 'Enable IPC server to handle input; otherwise enable sshkeyboard') parser.add_argument('--pass-through', action='store_true', help='Enable passthrough mode (use real-world view in XR device)') - parser.add_argument('--img-server-ip', type=str, default='192.168.123.164', help='IP address of image server') + parser.add_argument('--img-server-ip', type=str, default='127.0.0.1', help='IP address of image server') parser.add_argument('--affinity', action = 'store_true', help = 'Enable high priority and set CPU affinity mode') # record mode and task info parser.add_argument('--record', action = 'store_true', help = 'Enable data recording mode') @@ -113,15 +112,17 @@ if __name__ == '__main__': # image client img_client = ImageClient(host=args.img_server_ip) - xr_need_local_img = not (args.pass_through or img_client.enable_head_webrtc()) + camera_config = img_client.get_cam_config() + logger_mp.debug(f"Camera config: {camera_config}") + xr_need_local_img = not (args.pass_through or camera_config['head_camera']['enable_webrtc']) # televuer_wrapper: obtain hand pose data from the XR device and transmit the robot's head camera image to the XR device. tv_wrapper = TeleVuerWrapper(use_hand_tracking=args.xr_mode == "hand", pass_through=args.pass_through, - binocular=img_client.head_is_binocular(), - img_shape=img_client.get_head_shape(), - webrtc=img_client.enable_head_webrtc(), - webrtc_url=img_client.head_webrtc_url()) + binocular=camera_config['head_camera']['binocular'], + img_shape=camera_config['head_camera']['image_shape'], + webrtc=camera_config['head_camera']['enable_webrtc'], + webrtc_url=f"https://{args.img_server_ip}:{camera_config['head_camera']['webrtc_port']}/offer") # arm if args.arm == "G1_29": @@ -221,25 +222,26 @@ if __name__ == '__main__': READY = True # now ready to (1) enter START state while not START and not STOP: # wait for start or stop signal. time.sleep(0.033) - if img_client.enable_head_zmq() and xr_need_local_img: + if camera_config['head_camera']['enable_zmq'] and xr_need_local_img: head_img, _ = img_client.get_head_frame() + print(f"head_img: {type(head_img)}, {head_img.shape if head_img is not None else None}") tv_wrapper.render_to_xr(head_img) - logger_mp.info("start program.") + logger_mp.info("---------------------🚀start program🚀-------------------------") arm_ctrl.speed_gradual_max() # main loop. robot start to follow VR user's motion while not STOP: start_time = time.time() # get image - if img_client.enable_head_zmq(): + if camera_config['head_camera']['enable_zmq']: if args.record or xr_need_local_img: head_img, head_img_fps = img_client.get_head_frame() if xr_need_local_img: tv_wrapper.render_to_xr(head_img) - if img_client.enable_left_wrist_zmq(): + if camera_config['left_wrist_camera']['enable_zmq']: if args.record: left_wrist_img, _ = img_client.get_left_wrist_frame() - if img_client.enable_right_wrist_zmq(): + if camera_config['right_wrist_camera']['enable_zmq']: if args.record: right_wrist_img, _ = img_client.get_right_wrist_frame() @@ -356,19 +358,37 @@ if __name__ == '__main__': if RECORD_RUNNING: colors = {} depths = {} - if img_client.head_is_binocular(): - colors[f"color_{0}"] = head_img[:, :img_client.get_head_shape()[1]//2] - colors[f"color_{1}"] = head_img[:, img_client.get_head_shape()[1]//2:] - if img_client.enable_left_wrist_zmq(): - colors[f"color_{2}"] = left_wrist_img - if img_client.enable_right_wrist_zmq(): - colors[f"color_{3}"] = right_wrist_img + if camera_config['head_camera']['binocular']: + if head_img is not None: + colors[f"color_{0}"] = head_img[:, :camera_config['head_camera']['image_shape'][1]//2] + colors[f"color_{1}"] = head_img[:, camera_config['head_camera']['image_shape'][1]//2:] + else: + logger_mp.warning("Head image is None!") + if camera_config['left_wrist_camera']['enable_zmq']: + if left_wrist_img is not None: + colors[f"color_{2}"] = left_wrist_img + else: + logger_mp.warning("Left wrist image is None!") + if camera_config['right_wrist_camera']['enable_zmq']: + if right_wrist_img is not None: + colors[f"color_{3}"] = right_wrist_img + else: + logger_mp.warning("Right wrist image is None!") else: - colors[f"color_{0}"] = head_img - if img_client.enable_left_wrist_zmq(): - colors[f"color_{1}"] = left_wrist_img - if img_client.enable_right_wrist_zmq(): - colors[f"color_{2}"] = right_wrist_img + if head_img is not None: + colors[f"color_{0}"] = head_img + else: + logger_mp.warning("Head image is None!") + if camera_config['left_wrist_camera']['enable_zmq']: + if left_wrist_img is not None: + colors[f"color_{1}"] = left_wrist_img + else: + logger_mp.warning("Left wrist image is None!") + if camera_config['right_wrist_camera']['enable_zmq']: + if right_wrist_img is not None: + colors[f"color_{2}"] = right_wrist_img + else: + logger_mp.warning("Right wrist image is None!") states = { "left_arm": { "qpos": left_arm_state.tolist(), # numpy.array -> list diff --git a/teleop/utils/episode_writer.py b/teleop/utils/episode_writer.py index 89ec06f..7fe79af 100644 --- a/teleop/utils/episode_writer.py +++ b/teleop/utils/episode_writer.py @@ -72,7 +72,7 @@ class EpisodeWriter(): "depth": {"width":self.image_size[0], "height":self.image_size[1], "fps":self.frequency}, "audio": {"sample_rate": 16000, "channels": 1, "format":"PCM", "bits":16}, # PCM_S16 "joint_names":{ - "left_arm": ['kLeftShoulderPitch' ,'kLeftShoulderRoll', 'kLeftShoulderYaw', 'kLeftElbow', 'kLeftWristRoll', 'kLeftWristPitch', 'kLeftWristyaw'], + "left_arm": [], "left_ee": [], "right_arm": [], "right_ee": [], diff --git a/teleop/utils/motion_switcher.py b/teleop/utils/motion_switcher.py index 067d641..bd33f59 100644 --- a/teleop/utils/motion_switcher.py +++ b/teleop/utils/motion_switcher.py @@ -9,7 +9,7 @@ import time class MotionSwitcher: def __init__(self): self.msc = MotionSwitcherClient() - self.msc.SetTimeout(5.0) + self.msc.SetTimeout(1.0) self.msc.Init() def Enter_Debug_Mode(self): @@ -27,7 +27,7 @@ class MotionSwitcher: class LocoClientWrapper: def __init__(self): self.client = LocoClient() - self.client.SetTimeout(1.0) + self.client.SetTimeout(0.0001) self.client.Init() def Enter_Damp_Mode(self):