Explain this code [code]
- User
Explain this code
import time import argparse import cv2 from multiprocessing import shared_memory, Array, Lock import threading import os import sys current_dir = os.path.dirname(os.path.abspath(__file__)) parent_dir = os.path.dirname(current_dir) sys.path.append(parent_dir) from teleop.open_television.tv_wrapper import TeleVisionWrapper from teleop.robot_control.robot_arm import G1_29_ArmController from teleop.robot_control.robot_arm_ik import G1_29_ArmIK from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller from teleop.image_server.image_client import ImageClient from teleop.utils.episode_writer import EpisodeWriter if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument('--task_dir', type=str, default='data', help='path to save data') parser.add_argument('--frequency', type=int, default=30.0, help='save data\'s frequency') parser.add_argument('--record', action='store_true', help='Save data or not') parser.add_argument('--no-record', dest='record', action='store_false', help='Do not save data') parser.set_defaults(record=False) parser.add_argument('--binocular', action='store_true', help='Use binocular camera') parser.add_argument('--monocular', dest='binocular', action='store_false', help='Use monocular camera') parser.set_defaults(binocular=True) args = parser.parse_args() print(f"args:{args}\n") # image img_shape = (720, 2560, 3) img_shm = shared_memory.SharedMemory(create=True, size=np.prod(img_shape) * np.uint8().itemsize) img_array = np.ndarray(img_shape, dtype=np.uint8, buffer=img_shm.buf) img_client = ImageClient(img_shape = img_shape, img_shm_name = img_shm.name) image_receive_thread = threading.Thread(target=img_client.receive_process, daemon=True) image_receive_thread.daemon = True image_receive_thread.start() # television and arm tv_wrapper = TeleVisionWrapper(args.binocular, img_shape, img_shm.name) arm_ctrl = G1_29_ArmController() arm_ik = G1_29_ArmIK() # hand left_hand_array = Array('d', 75, lock=True) # [input] right_hand_array = Array('d', 75, lock=True) # [input] dual_hand_data_lock = Lock() dual_hand_state_array = Array('d', 14, lock=False) # [output] current left, right hand state(14) data. dual_hand_action_array = Array('d', 14, lock=False) # [output] current left, right hand action(14) data. hand_ctrl = Dex3_1_Controller(left_hand_array, right_hand_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array) if args.record: recorder = EpisodeWriter(task_dir=args.task_dir, frequency=args.frequency) try: user_input = input("Please enter the start signal (enter 'r' to start the subsequent program):\n") if user_input.lower() == 'r': arm_ctrl.speed_gradual_max() if args.record: press_key_s_count = 0 running = True recording = False while running: start_time = time.time() head_rmat, left_wrist, right_wrist, left_hand, right_hand = tv_wrapper.get_data() # send hand skeleton data to hand_ctrl.control_process left_hand_array[:] = left_hand.flatten() right_hand_array[:] = right_hand.flatten() # get current state data. current_lr_arm_q = arm_ctrl.get_current_dual_arm_q() current_lr_arm_dq = arm_ctrl.get_current_dual_arm_dq() # solve ik using motor data and wrist pose, then use ik results to control arms. time_ik_start = time.time() sol_q, sol_tauff = arm_ik.solve_ik(left_wrist, right_wrist, current_lr_arm_q, current_lr_arm_dq) time_ik_end = time.time() # print(f"ik:\t{round(time_ik_end - time_ik_start, 6)}") arm_ctrl.ctrl_dual_arm(sol_q, sol_tauff) resized_image = cv2.resize(img_array, (img_shape[1] // 2, img_shape[0] // 2)) cv2.imshow("record image", resized_image) key = cv2.waitKey(1) & 0xFF if key == ord('q'): running = False elif key == ord('s') and args.record: press_key_s_count += 1 if press_key_s_count % 2 == 1: print("==> Start recording...") recording = True recorder.create_episode() print("==> Create episode ok.") else: print("==> End recording...") recording = False recorder.save_episode() print("==> Save episode ok.") # record data if args.record: 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:] current_image = img_array.copy() left_arm_state = current_lr_arm_q[:7] right_arm_state = current_lr_arm_q[-7:] left_arm_action = sol_q[:7] right_arm_action = sol_q[-7:] if recording: colors = {} depths = {} if args.binocular: colors[f"color_{0}"] = current_image[:, :img_shape[1]//2] colors[f"color_{1}"] = current_image[:, img_shape[1]//2:] else: colors[f"color_{0}"] = current_image states = { "left_arm": { "qpos": left_arm_state.tolist(), # numpy.array -> list "qvel": [], "torque": [], }, "right_arm": { "qpos": right_arm_state.tolist(), "qvel": [], "torque": [], }, "left_hand": { "qpos": left_hand_state, "qvel": [], "torque": [], }, "right_hand": { "qpos": right_hand_state, "qvel": [], "torque": [], }, "body": None, } actions = { "left_arm": { "qpos": left_arm_action.tolist(), "qvel": [], "torque": [], }, "right_arm": { "qpos": right_arm_action.tolist(), "qvel": [], "torque": [], }, "left_hand": { "qpos": left_hand_action, "qvel": [], "torque": [], }, "right_hand": { "qpos": right_hand_action, "qvel": [], "torque": [], }, "body": None, } recorder.add_item(colors=colors, depths=depths, states=states, actions=actions) current_time = time.time() time_elapsed = current_time - start_time sleep_time = max(0, (1 / float(args.frequency)) - time_elapsed) time.sleep(sleep_time) # print(f"main process sleep: {sleep_time}") except KeyboardInterrupt: print("KeyboardInterrupt, exiting program...") finally: img_shm.close() img_shm.unlink() print("Finally, exiting program...") exit(0)``` - Assistant
Certainly! Let's break down and explain the provided Python script step by step. This script appears to be part of a teleoperation system that integrates image processing, robot arm control, and data recording functionalities. Here's a comprehensive overview of its structure and functionality:
1. Import Statements
import numpy as np import time import argparse import cv2 from multiprocessing import shared_memory, Array, Lock import threading import os import sys current_dir = os.path.dirname(os.path.abspath(__file__)) parent_dir = os.path.dirname(current_dir) sys.path.append(parent_dir) from teleop.open_television.tv_wrapper import TeleVisionWrapper from teleop.robot_control.robot_arm import G1_29_ArmController from teleop.robot_control.robot_arm_ik import G1_29_ArmIK from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller from teleop.image_server.image_client import ImageClient from teleop.utils.episode_writer import EpisodeWriterExplanation:
-
Standard Libraries:
numpy,time,argparse,cv2: For numerical operations, time management, command-line argument parsing, and computer vision tasks respectively.multiprocessing.shared_memory,Array,Lock: For inter-process communication and shared data structures.threading: To handle concurrent execution threads.os,sys: For interacting with the operating system and Python runtime environment.
-
Path Configuration:
- The script determines its current directory and appends the parent directory to
sys.pathto allow importing modules from theteleoppackage.
- The script determines its current directory and appends the parent directory to
-
Custom Modules from
teleopPackage:TeleVisionWrapper: Likely handles interactions with a television or display system.G1_29_ArmController&G1_29_ArmIK: Manage robot arm control and inverse kinematics computations.Dex3_1_Controller: Controls the robot’s hand, possibly a dexterous hand unit.ImageClient: Handles image acquisition from an image server.EpisodeWriter: Manages recording and saving episodes of data.
2. Main Execution Block
if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument('--task_dir', type=str, default='data', help='path to save data') parser.add_argument('--frequency', type=int, default=30.0, help='save data\'s frequency') parser.add_argument('--record', action='store_true', help='Save data or not') parser.add_argument('--no-record', dest='record', action='store_false', help='Do not save data') parser.set_defaults(record=False) parser.add_argument('--binocular', action='store_true', help='Use binocular camera') parser.add_argument('--monocular', dest='binocular', action='store_false', help='Use monocular camera') parser.set_defaults(binocular=True) args = parser.parse_args() print(f"args:{args}\n")Explanation:
-
Argument Parsing:
--task_dir: Directory where data will be saved (default:'data').--frequency: Frequency at which data is saved (default:30Hz).--record/--no-record: Flags to enable or disable data recording (default:False).--binocular/--monocular: Choose between binocular (stereo) and monocular (single) camera setups (default:Truefor binocular).
-
Parsing and Displaying Arguments:
- The script parses the command-line arguments and prints them for confirmation.
3. Setting Up Shared Memory for Images
# image img_shape = (720, 2560, 3) img_shm = shared_memory.SharedMemory(create=True, size=np.prod(img_shape) * np.uint8().itemsize) img_array = np.ndarray(img_shape, dtype=np.uint8, buffer=img_shm.buf) img_client = ImageClient(img_shape = img_shape, img_shm_name = img_shm.name) image_receive_thread = threading.Thread(target=img_client.receive_process, daemon=True) image_receive_thread.daemon = True image_receive_thread.start()Explanation:
-
Image Dimensions:
img_shape: Specifies the image size as 720 pixels in height, 2560 pixels in width, and 3 color channels (likely RGB).
-
Shared Memory Setup:
SharedMemory: Allocates a shared memory block large enough to hold the image data (720 * 2560 * 3bytes foruint8type).img_array: Creates a NumPy array that maps to the shared memory buffer, facilitating image data access and manipulation.
-
Image Client and Threading:
ImageClient: Initialized with image shape and shared memory name to handle image reception from an image server.image_receive_thread: Starts a daemon thread that continuously receives and processes images using thereceive_processmethod fromImageClient. The daemon thread ensures it doesn’t prevent the program from exiting.
4. Initializing Teleoperation Components
# television and arm tv_wrapper = TeleVisionWrapper(args.binocular, img_shape, img_shm.name) arm_ctrl = G1_29_ArmController() arm_ik = G1_29_ArmIK() # hand left_hand_array = Array('d', 75, lock=True) # [input] right_hand_array = Array('d', 75, lock=True) # [input] dual_hand_data_lock = Lock() dual_hand_state_array = Array('d', 14, lock=False) # [output] current left, right hand state (14) data. dual_hand_action_array = Array('d', 14, lock=False) # [output] current left, right hand action (14) data. hand_ctrl = Dex3_1_Controller(left_hand_array, right_hand_array, dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array)Explanation:
-
TeleVisionWrapper:- Initialized with camera type (
binocularormonocular), image shape, and shared memory name to handle display or vision-related functionalities.
- Initialized with camera type (
-
Robot Arm Components:
G1_29_ArmController: Manages the physical robot arm’s movements and commands.G1_29_ArmIK: Handles inverse kinematics calculations to determine joint positions based on desired end-effector positions.
-
Robot Hand Components:
- Shared Arrays for Hand Data:
left_hand_array&right_hand_array: Shared memory arrays of 75 double-precision floats each, used as inputs for hand control data.dual_hand_state_array&dual_hand_action_array: Shared memory arrays of 14 double-precision floats each, used to output the current state and actions of both hands (7 elements each for left and right hands).
- Locks:
dual_hand_data_lock: Ensures thread-safe access to shared hand data.
Dex3_1_Controller: Controls the robot hands using the shared arrays and lock to manage state and actions.
- Shared Arrays for Hand Data:
5. Setting Up the Recorder (Optional)
if args.record: recorder = EpisodeWriter(task_dir=args.task_dir, frequency=args.frequency)Explanation:
EpisodeWriter:- If data recording is enabled (
--recordflag), anEpisodeWriterinstance is created to handle saving data episodes to the specifiedtask_dirat the definedfrequency.
- If data recording is enabled (
6. Main Execution Loop
try: user_input = input("Please enter the start signal (enter 'r' to start the subsequent program):\n") if user_input.lower() == 'r': arm_ctrl.speed_gradual_max() if args.record: press_key_s_count = 0 running = True recording = False while running: start_time = time.time() head_rmat, left_wrist, right_wrist, left_hand, right_hand = tv_wrapper.get_data() # send hand skeleton data to hand_ctrl.control_process left_hand_array[:] = left_hand.flatten() right_hand_array[:] = right_hand.flatten() # get current state data. current_lr_arm_q = arm_ctrl.get_current_dual_arm_q() current_lr_arm_dq = arm_ctrl.get_current_dual_arm_dq() # solve ik using motor data and wrist pose, then use ik results to control arms. time_ik_start = time.time() sol_q, sol_tauff = arm_ik.solve_ik(left_wrist, right_wrist, current_lr_arm_q, current_lr_arm_dq) time_ik_end = time.time() # print(f"ik:\t{round(time_ik_end - time_ik_start, 6)}") arm_ctrl.ctrl_dual_arm(sol_q, sol_tauff) resized_image = cv2.resize(img_array, (img_shape[1] // 2, img_shape[0] // 2)) cv2.imshow("record image", resized_image) key = cv2.waitKey(1) & 0xFF if key == ord('q'): running = False elif key == ord('s') and args.record: press_key_s_count += 1 if press_key_s_count % 2 == 1: print("==> Start recording...") recording = True recorder.create_episode() print("==> Create episode ok.") else: print("==> End recording...") recording = False recorder.save_episode() print("==> Save episode ok.") # record data if args.record: 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:] current_image = img_array.copy() left_arm_state = current_lr_arm_q[:7] right_arm_state = current_lr_arm_q[-7:] left_arm_action = sol_q[:7] right_arm_action = sol_q[-7:] if recording: colors = {} depths = {} if args.binocular: colors[f"color_{0}"] = current_image[:, :img_shape[1]//2] colors[f"color_{1}"] = current_image[:, img_shape[1]//2:] else: colors[f"color_{0}"] = current_image states = { "left_arm": { "qpos": left_arm_state.tolist(), # numpy.array -> list "qvel": [], "torque": [], }, "right_arm": { "qpos": right_arm_state.tolist(), "qvel": [], "torque": [], }, "left_hand": { "qpos": left_hand_state, "qvel": [], "torque": [], }, "right_hand": { "qpos": right_hand_state, "qvel": [], "torque": [], }, "body": None, } actions = { "left_arm": { "qpos": left_arm_action.tolist(), "qvel": [], "torque": [], }, "right_arm": { "qpos": right_arm_action.tolist(), "qvel": [], "torque": [], }, "left_hand": { "qpos": left_hand_action, "qvel": [], "torque": [], }, "right_hand": { "qpos": right_hand_action, "qvel": [], "torque": [], }, "body": None, } recorder.add_item(colors=colors, depths=depths, states=states, actions=actions) current_time = time.time() time_elapsed = current_time - start_time sleep_time = max(0, (1 / float(args.frequency)) - time_elapsed) time.sleep(sleep_time) # print(f"main process sleep: {sleep_time}") except KeyboardInterrupt: print("KeyboardInterrupt, exiting program...") finally: img_shm.close() img_shm.unlink() print("Finally, exiting program...") exit(0)Explanation:
a. User Input to Start Execution:
- The script waits for the user to input the start signal (
'r'). Upon receiving'r', it proceeds to initialize and start the main loop.
user_input = input("Please enter the start signal (enter 'r' to start the subsequent program):\n") if user_input.lower() == 'r': arm_ctrl.speed_gradual_max() if args.record: press_key_s_count = 0arm_ctrl.speed_gradual_max(): Likely ramps up the arm's speed to its maximum in preparation for operation.press_key_s_count: Initializes a counter to manage recording toggles when the user presses the's'key.
b. Main Loop (
while running):The loop continues to execute as long as
runningisTrue.-
Timing Management:
start_time = time.time()- Records the start time of each loop iteration to manage the execution frequency.
-
Data Acquisition from
TeleVisionWrapper:head_rmat, left_wrist, right_wrist, left_hand, right_hand = tv_wrapper.get_data()- Retrieves the head rotation matrix, wrist positions, and hand data from the
TeleVisionWrapper.
- Retrieves the head rotation matrix, wrist positions, and hand data from the
-
Updating Hand Data for
Dex3_1_Controller:left_hand_array[:] = left_hand.flatten() right_hand_array[:] = right_hand.flatten()- Flattens and updates the shared arrays with the latest hand data, making them available for the hand controller.
-
Fetching Current Arm State:
current_lr_arm_q = arm_ctrl.get_current_dual_arm_q() current_lr_arm_dq = arm_ctrl.get_current_dual_arm_dq()- Retrieves the current joint positions (
q) and velocities (dq) of both arms.
- Retrieves the current joint positions (
-
Inverse Kinematics (IK) Solutions:
sol_q, sol_tauff = arm_ik.solve_ik(left_wrist, right_wrist, current_lr_arm_q, current_lr_arm_dq) arm_ctrl.ctrl_dual_arm(sol_q, sol_tauff)arm_ik.solve_ik: Computes the desired joint positions (sol_q) and torques (sol_tauff) based on wrist positions and current arm states.arm_ctrl.ctrl_dual_arm: Commands the robot arms to move based on the IK solutions.
-
Image Display and User Input Handling:
resized_image = cv2.resize(img_array, (img_shape[1] // 2, img_shape[0] // 2)) cv2.imshow("record image", resized_image) key = cv2.waitKey(1) & 0xFF if key == ord('q'): running = False elif key == ord('s') and args.record: # Toggle recording- Display:
- Resizes the image from shared memory to half its width and height for display efficiency.
- Uses OpenCV to show the image in a window titled
"record image".
- User Controls:
- Pressing
'q': SetsrunningtoFalseto exit the loop and terminate the program. - Pressing
's'(if recording is enabled): Toggles recording on and off, creating or saving episodes accordingly.
- Pressing
- Display:
-
Recording Data (If Enabled):
if args.record: 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:] current_image = img_array.copy() left_arm_state = current_lr_arm_q[:7] right_arm_state = current_lr_arm_q[-7:] left_arm_action = sol_q[:7] right_arm_action = sol_q[-7:] if recording: # Prepare data structures recorder.add_item(colors=colors, depths=depths, states=states, actions=actions)-
Data Acquisition:
- Locks access to shared hand state and action arrays to prevent data races.
- Copies the current image from shared memory.
- Splits arm states and actions into left and right components.
-
Data Structuring for Recording:
- Colors and Depths:
- If using a binocular camera, splits the image into left and right color streams.
- If using a monocular camera, only one color stream is recorded.
- States:
left_arm&right_arm: Position (qpos) data of the arms.left_hand&right_hand: State data of the hands.
- Actions:
- Similar structure to states, capturing the actions taken by arms and hands.
- Colors and Depths:
-
Recording the Data:
recorder.add_item: Saves the prepared data structures into the current episode.
-
-
Maintaining Execution Frequency:
current_time = time.time() time_elapsed = current_time - start_time sleep_time = max(0, (1 / float(args.frequency)) - time_elapsed) time.sleep(sleep_time)- Calculates the time taken for the current loop iteration.
- Determines the remaining time to sleep to maintain the desired frequency (
args.frequency). - Sleeps accordingly to ensure consistent loop timing.
c. Handling Keyboard Interrupts and Cleanup:
except KeyboardInterrupt: print("KeyboardInterrupt, exiting program...") finally: img_shm.close() img_shm.unlink() print("Finally, exiting program...") exit(0)- Graceful Shutdown:
- Catches
KeyboardInterrupt(e.g., when the user pressesCtrl+C) to print a message before exiting. img_shm.close()&img_shm.unlink(): Closes and unlinks the shared memory to free resources.- Prints a final message before terminating the program.
- Catches
7. Summary of Functionality
-
Initialization:
- Sets up shared memory for image data.
- Initializes image reception via a separate thread.
- Configures robot arm and hand controllers.
- Optionally sets up data recording.
-
User Interaction:
- Waits for user input (
'r') to start the operation.
- Waits for user input (
-
Operation Loop:
- Continuously acquires sensor data (e.g., camera images, hand states).
- Updates robot hand commands based on user inputs or other data sources.
- Computes inverse kinematics to determine appropriate joint movements for the robot arms.
- Controls the robot arms accordingly.
- Displays live camera feed and handles user inputs for quitting or toggling recording.
- Records data episodes if enabled, storing relevant sensor states and actions.
-
Termination:
- Ensures all resources are properly released upon exiting, whether normally or via an interrupt.
8. Potential Use Cases
-
Teleoperation Systems:
- Remote control of robotic arms and hands, possibly for tasks like assembly, surgery, or hazardous environment operations.
-
Data Collection and Analysis:
- Recording episodes of robot interactions for machine learning, performance analysis, or debugging purposes.
-
Human-Robot Interaction:
- Integrating human inputs (e.g., gestures captured via cameras) to control robotic actuators in real-time.
9. Considerations and Best Practices
-
Thread Safety:
- The use of locks (
dual_hand_data_lock) ensures that shared data structures are accessed safely across threads and processes.
- The use of locks (
-
Resource Management:
- Proper closing and unlinking of shared memory prevent memory leaks and ensure that system resources are freed upon program termination.
-
Modularity:
- The script leverages modular components (e.g.,
TeleVisionWrapper,ArmController) to separate concerns and enhance maintainability.
- The script leverages modular components (e.g.,
-
User Feedback:
- Informative print statements guide the user through the program’s state transitions, such as starting or stopping recordings.
-
Error Handling:
- The
try-except-finallyblock ensures that even if an error occurs or the user interrupts the program, cleanup processes are executed.
- The
10. Dependencies and Requirements
To ensure the script runs correctly, the following dependencies and prerequisites should be met:
-
Python Version:
- Compatible with Python 3.7 or later, due to the use of
multiprocessing.shared_memory.
- Compatible with Python 3.7 or later, due to the use of
-
Required Libraries:
numpy,opencv-python(cv2), and other standard Python libraries.
-
Custom Modules:
- Ensure that the
teleoppackage and its submodules (open_television,robot_control,image_server,utils) are correctly installed and accessible.
- Ensure that the
-
Hardware Interfaces:
- Properly configured robot arms, hands, cameras, and their respective drivers or APIs to interface with the software.
Conclusion
This script orchestrates a complex teleoperation system by integrating image processing, robot control, and data recording. It leverages multiprocessing and threading to handle concurrent tasks efficiently, ensuring real-time performance and responsiveness. Proper resource management and user interaction handling make it robust and user-friendly for applications requiring precise control and data management of robotic systems.
-