使用示例

Basic Usage Pattern

import time
import numpy as np
from mozrobot import MOZ1Robot, MOZ1RobotConfig

# 1. Create configuration
config = MOZ1RobotConfig(
    realsense_serials="012345678,012345678,012345678",
    structure="wholebody",
    robot_control_hz=120
)

# 2. Create robot instance
robot = MOZ1Robot(config)

try:
    # 3. Connect to robot
    robot.connect()

    if robot.is_robot_connected:
        print("Robot connected successfully!")

    # 4. Enable external control mode (required)
    robot.enable_external_following_mode()

    # 5. Get current state
    obs = robot.capture_observation()
    print(f"Left arm position: {obs['leftarm_state_cart_pos']}")

    # 6. Send control commands
    dt = 1.0 / robot.control_hz
    # Omit trajectory planning...
    action_list = waypoints_list()
    for action in action_list:
        action = {
            "leftarm_cmd_cart_pos": action["leftarm_action"],
            "rightarm_cmd_cart_pos": action["rightarm_action"]
        }
        robot.send_action(action)
        time.sleep(dt)

except Exception as e:
    print(f"Error: {e}")
finally:
    # 7. Disconnect
    robot.disconnect()

Context Manager Pattern

from mozrobot import MOZ1Robot, MOZ1RobotConfig

config = MOZ1RobotConfig(structure="dualarm")

# Automatic connection and disconnection management
with MOZ1Robot(config) as robot:
    # Enable external control mode (required)
    robot.enable_external_following_mode()

    # Get robot state
    obs = robot.capture_observation()

Trajectory Control Example

import time
import numpy as np
from mozrobot import MOZ1Robot, MOZ1RobotConfig

def generate_circular_trajectory(center, radius, num_points):
    """Generate circular trajectory points."""
    angles = np.linspace(0, 2*np.pi, num_points)
    trajectory = []
    for angle in angles:
        x = center[0] + radius * np.cos(angle)
        y = center[1] + radius * np.sin(angle)
        z = center[2]
        trajectory.append([x, y, z, 0, 0, 0])  # position + orientation
    return np.array(trajectory)

config = MOZ1RobotConfig(structure="dualarm")

with MOZ1Robot(config) as robot:
    # Enable external control mode (required)
    robot.enable_external_following_mode()

    # Get current position
    obs = robot.capture_observation()
    current_pos = obs["leftarm_state_cart_pos"]

    # Generate circular trajectory
    center = current_pos[:3]  # Use current position as center
    trajectory = generate_circular_trajectory(center, 0.05, 60)  # 5cm radius, 60 points

    # Execute trajectory
    dt = 1.0 / robot.control_hz
    for waypoint in trajectory:
        action = {"leftarm_cmd_cart_pos": waypoint}
        robot.send_action(action)
        time.sleep(dt)