Add trajectory recording/replay system with performance improvements

[LeonLiu4]

What's Added:
Trajectory Recording - Press 'r' to start/stop recording robot movements
Trajectory Replay - Replay recorded trajectories with replay_with_viewer.py

How to Use:
Run teleop: python examples/run_pick_cube_teleop.py
Press 'r' to record, 'r' again to stop and save
Replay: python examples/replay_with_viewer.py

[Copilot]

Pull Request Overview

This PR adds a comprehensive trajectory recording and replay system with performance improvements. The system allows users to record robot movements using keyboard teleop controls and replay them later for demonstration or analysis purposes.

• Adds trajectory recording capability with 'r' key toggle in the teleop interface
• Implements trajectory replay functionality with a dedicated replay script
• Includes performance optimizations and bug fixes across multiple robot control components

Reviewed Changes

Copilot reviewed 8 out of 8 changed files in this pull request and generated 3 comments.

Show a summary per file

File	Description
src/env/gs_env/sim/robots/manipulators.py	Adds reset_to_pose method, fixes inverse kinematics handling, and improves error handling
src/env/gs_env/sim/envs/manipulation/pick_cube_env.py	Updates joint configuration to include 7th joint for proper robot initialization
src/env/gs_env/common/utils/gs_utils.py	Fixes return type annotation for better type safety
src/agent/gs_agent/wrappers/teleop_wrapper.py	Major refactor adding trajectory recording/replay system and improved keyboard handling
src/agent/gs_agent/wrappers/gs_env_wrapper.py	Updates type annotation for better compatibility
examples/run_pick_cube_teleop.py	Updates teleop script with new recording instructions and improved control flow
examples/replay_with_viewer.py	New replay script for trajectory playback using recorded data
README.md	Comprehensive documentation update with teleop usage instructions and trajectory format details

Comments suppressed due to low confidence (1)

src/env/gs_env/sim/robots/manipulators.py:227

• The code now assumes inverse_kinematics always returns a tuple and never returns None, but there's no error handling if the IK solver fails. Consider adding a check to handle cases where q_pos might be None or invalid.

        q_pos, _ = self._robot_entity.inverse_kinematics(
            link=self._ee_link,
            pos=target_pos,
            quat=target_quat,
            dofs_idx_local=self._arm_dof_idx,
            max_samples=10,  # number of IK samples
            max_solver_iters=20,  # maximum solver iterations
        )
        q_pos[:, self._fingers_dof] = torch.tensor(
            [act.gripper_width, act.gripper_width], device=self._device
        )
        self._robot_entity.control_dofs_position(position=q_pos)

---

_{Tip: Customize your code reviews with copilot-instructions.md. Create the file or learn how to get started.}