运动控制

本模块提供机器人运动控制相关的所有功能,包括站立、躺下、姿态控制、电池状态查询等。

核心功能

站立与躺下

stand_up(timeout=30)

Make the robot stand up.

This function intelligently checks the current robot state before sending commands. - If already standing: returns success immediately without sending command - If currently standing up: returns failure (wait for completion) - If lying down: sends stand up command

Parameters:

timeout (int) – Timeout in seconds for the service call (default: 5)

Returns:

Response containing state and result

  • state.code: StateCode (0=success, 1=fail, etc.)

  • state.describe: Description of the result

  • response.result: Boolean indicating if command executed successfully

Return type:

StandUpResponse

Examples:

# Make the robot stand up
result = stand_up()
if result.response.result:
    print("Robot stood up successfully")
else:
    print(f"Failed: {result.state.describe}")

# Stand up with custom timeout
result = stand_up(timeout=10)
if result.response.result:
    print("Robot is now standing")

Note

  • The robot driver must be enabled before calling this function

lie_down(timeout=30)

Make the robot lie down.

This function intelligently checks the current robot state before sending commands. - If already lying down: returns success immediately without sending command - If currently standing up: returns failure (cannot interrupt) - If standing: sends lie down command

Parameters:

timeout (int) – Timeout in seconds for the service call (default: 5)

Returns:

Response containing state and result

  • state.code: StateCode (0=success, 1=fail, etc.)

  • state.describe: Description of the result

  • response.result: Boolean indicating if command executed successfully

Return type:

LieDownResponse

Examples:

# Make the robot lie down
result = lie_down()
if result.response.result:
    print("Robot is now lying down")
else:
    print(f"Failed: {result.state.describe}")

# Lie down with custom timeout
result = lie_down(timeout=10)
if result.response.result:
    print("Robot successfully lying down")

Note

  • The robot driver must be enabled before calling this function

get_robot_state()

Get the current robot posture state.

This function returns the robot’s current state with automatic data validation. If the state data is stale or not available, it returns UNKNOWN_STATE.

Returns:

Response containing state and robot_state

  • state: State object with code and describe

  • robot_state:bv RobotState enum value

    • RobotState.LIE_DOWN (0): Lying down

    • RobotState.STANDING_UP (1): Currently standing up (in transition)

    • RobotState.STAND_UP (2): Standing

    • RobotState.UNKNOWN_STATE (7): Unknown/No state (data unavailable or stale)

Return type:

GetRobotStateResponse

Examples:

# Get robot state
response = get_robot_state()

# Check if successful
if response.state.code == 0:  # StateCode.success
    if response.robot_state == RobotState.STAND_UP:
        print("Robot is standing")
    elif response.robot_state == RobotState.LIE_DOWN:
        print("Robot is lying down")
    elif response.robot_state == RobotState.UNKNOWN_STATE:
        print("Robot state unknown (data unavailable)")

# Simple usage
state = get_robot_state().robot_state
if state == RobotState.STAND_UP:
    print("Standing")

# Get integer value if needed
state_value = int(get_robot_state().robot_state)
print(f"State value: {state_value}")

Note

  • Data from /nav/robot_state topic subscription (5 second timeout)

  • Returns UNKNOWN_STATE if data is stale or not yet received

  • Data validation is performed automatically

  • stand_up() and lie_down() functions also use this validation internally

身体控制

adjust_body_height(percent, timeout=5)

Adjust the robot body height.

Parameters:

  • percent (int) – Height percentage (20-50)

  • timeout (int) – Timeout in seconds for the service call (default: 5)

Returns:

Response containing state and result

  • state.code: StateCode (0=success, 1=fail, etc.)

  • state.describe: Description of the result

  • response.result: Boolean indicating if command executed successfully

Return type:

RobotCommandResponse

Examples:

# Adjust height to 30%
result = adjust_body_height(30)
if result.response.result:
    print("Height adjusted successfully")

Note

  • Valid height range is 20-50

  • Robot must be standing to adjust height

set_gait(gait_type, timeout=5)

Set the robot gait type.

Parameters:

  • gait_type (GaitType) – Gait type enum - GaitType.TROT (0): Trot gait - GaitType.NORMAL_STAIR (1): Normal stair gait - GaitType.SENSE_STAIR (6): Sense stair gait - GaitType.POSE_ADJUST (13): Pose adjust gait - GaitType.CAR_MODE (14): Pose adjust gait - GaitType.RL_TROT (0x20): RL trot gait - GaitType.RL_Mountain (0x21): RL mountain gait

  • timeout (int) – Timeout in seconds for the service call (default: 5)

Returns:

Response containing state and result

  • state.code: StateCode (0=success, 1=fail, etc.)

  • state.describe: Description of the result

  • response.result: Boolean indicating if command executed successfully

Return type:

RobotCommandResponse

Examples:

# Set to stairs gait
result = set_gait(GaitType.TROT)
if result.response.result:
    print("Gait set to stairs mode")

# Set to normal gait
result = set_gait(GaitType.NORMAL_GAIT)

Note

  • Robot must be standing to change gait

get_gait()

Get the current robot gait type.

This function returns the robot’s current gait type with automatic data validation. If the gait data is stale or not available, it returns UNKNOWN_GAIT.

Returns:

Response containing state and gait_type

  • state: State object with code and describe

  • gait_type: GaitType enum value

    • GaitType.TROT (0): Trot gait

    • GaitType.NORMAL_STAIR (1): Normal stair gait

    • GaitType.SLOPE (2): Slope stair gait

    • GaitType.SENSE_STAIR (6): Sense stair gait

    • GaitType.POSE_ADJUST (13): Pose adjust gait

    • GaitType.CAR_MODE (14): Pose adjust gait

    • GaitType.RL_TROT (0x20): RL trot gait

    • GaitType.RL_Mountain (0x21): RL mountain gait

    • GaitType.UNKNOWN_GAIT (99): Unknown (data unavailable or stale)

Return type:

GetGaitResponse

Examples:

# Get gait type
response = get_gait()
if response.state.code == 0:  # StateCode.success
    if response.gait_type == GaitType.STAIRS_GAIT:
        print("Stairs gait active")
    elif response.gait_type == GaitType.NORMAL_GAIT:
        print("Normal gait active")

Note

  • Data from /gait topic subscription (5 second timeout)

  • Returns UNKNOWN_GAIT if data is stale or not yet received

控制模式

set_control_mode(mode, timeout=5)

Set the robot control mode.

Parameters:

  • mode (ControlMode) – Control mode enum - ControlMode.JOY_MODE (0): Joystick/manual control mode - ControlMode.NAV_MODE (1): Navigation/automatic control mode

  • timeout (int) – Timeout in seconds for the service call (default: 5)

Returns:

Response containing state and result

  • state.code: StateCode (0=success, 1=fail, etc.)

  • state.describe: Description of the result

  • response.result: Boolean indicating if command executed successfully

Return type:

RobotCommandResponse

Examples:

# Set to navigation mode
result = set_control_mode(ControlMode.NAV_MODE)
if result.response.result:
    print("Control mode set to navigation")

# Set to joystick mode
result = set_control_mode(ControlMode.JOY_MODE)

Note

  • Switches between manual and automatic control

get_control_mode()

Get the current robot control mode.

This function returns the robot’s current control mode with automatic data validation. If the control mode data is stale or not available, it returns UNKNOWN_MODE.

Returns:

Response containing state and control_mode

  • state: State object with code and describe

  • control_mode: ControlMode enum value

    • ControlMode.JOY_MODE (0): Joystick/manual control

    • ControlMode.NAV_MODE (1): Navigation/automatic control

    • ControlMode.UNKNOWN_MODE (99): Unknown (data unavailable or stale)

Return type:

GetControlModeResponse

Examples:

# Get control mode
response = get_control_mode()
if response.state.code == 0:  # StateCode.success
    if response.control_mode == ControlMode.NAV_MODE:
        print("Navigation mode active")
    elif response.control_mode == ControlMode.JOY_MODE:
        print("Joystick mode active")

Note

  • Data from /nav/control_mode topic subscription (5 second timeout)

  • Returns UNKNOWN_MODE if data is stale or not yet received

充电功能

go_to_dock(timeout=60)

Command the robot to go to the charging dock.

Parameters:

timeout (int) – Timeout in seconds for the service call (default: 30)

Returns:

Response containing state and result

  • state.code: StateCode (0=success, 1=fail, etc.)

  • state.describe: Description of the result

  • response.result: Boolean indicating if command executed successfully

Return type:

GoToDockResponse

Examples:

# Command robot to go to dock
result = go_to_dock()
if result.response.result:
    print("Robot is going to dock")
else:
    print(f"Failed: {result.state.describe}")

# With custom timeout
result = go_to_dock(timeout=60)

Note

  • Uses ‘/nav/robot_command’ service with ‘GoToDock’ command

  • Robot must be in proper state to execute this command

leave_dock(timeout=60)

Command the robot to leave the charging dock.

Parameters:

timeout (int) – Timeout in seconds for the service call (default: 30)

Returns:

Response containing state and result

  • state.code: StateCode (0=success, 1=fail, etc.)

  • state.describe: Description of the result

  • response.result: Boolean indicating if command executed successfully

Return type:

LeaveDockResponse

Examples:

# Command robot to leave dock
result = leave_dock()
if result.response.result:
    print("Robot is leaving dock")
else:
    print(f"Failed: {result.state.describe}")

# With custom timeout
result = leave_dock(timeout=60)

Note

  • Uses ‘/nav/robot_command’ service with ‘LeaveDock’ command

  • Robot must be on the dock to execute this command

get_dock_state()

Get the current charging dock state.

This function returns the robot’s current dock state with automatic data validation. If the dock state data is stale or not available, it returns UNKNOWN_DOCK.

Returns:

Response containing state and dock_state

  • state: State object with code and describe

  • dock_state: DockState enum value

    • DockState.IDLE (0): Idle

    • DockState.GOING_TO_DOCK (1): Going to dock

    • DockState.STAND_ABOVE_DOCK (2): Standing above dock

    • DockState.ADJUST_POSE (3): Adjusting pose

    • DockState.LIE_ON_DOCKER (4): Lying on dock

    • DockState.STANDING_AFTER_CHARGING (5): Standing after charging

    • DockState.LEAVING_DOCK (6): Leaving dock

    • DockState.LEAVE_DOCK (7): Left dock

    • DockState.UNKNOWN_DOCK (99): Unknown (data unavailable or stale)

Return type:

GetDockStateResponse

Examples:

# Get dock state
response = get_dock_state()
if response.state.code == 0:  # StateCode.success
    if response.dock_state == DockState.LIE_ON_DOCKER:
        print("Robot is on dock and charging")
    elif response.dock_state == DockState.IDLE:
        print("Robot is idle")

Note

  • Data from /nav/dock_state topic subscription (5 second timeout)

  • Returns UNKNOWN_DOCK if data is stale or not yet received

电池状态

get_battery_state()

Get the complete battery state information.

This function returns the robot’s complete battery state with automatic data validation. If the battery state data is stale or not available, is_valid will be false.

Returns:

Response containing state and battery information

  • state: State object with code and describe

  • is_valid: Boolean indicating if battery_state data is valid

    • true: Data is valid and up-to-date

    • false: Data is stale, unavailable, or module not initialized

  • battery_state: sensor_msgs/BatteryState with complete battery information

    • percentage: Battery charge percentage (0.0-100.0)

    • voltage: Voltage in Volts (V)

    • current: Current in Amperes (A), negative indicates discharge

    • temperature: Temperature in Celsius (degC)

    • charge: Current charge in Amp-hours (Ah)

    • capacity: Battery capacity in Amp-hours (Ah)

    • power_supply_status: Charging status (0=UNKNOWN, 1=CHARGING, 2=DISCHARGING, 3=NOT_CHARGING, 4=FULL)

    • power_supply_health: Battery health status

    • power_supply_technology: Battery technology type

    • present: Battery present (true/false)

    • cell_voltage: Voltage of individual cells

    • location: Battery location/identifier

    • serial_number: Battery serial number

Return type:

GetBatteryStateResponse

Examples:

# Get complete battery state
response = get_battery_state()
if response.state.code == 0:  # StateCode.success
    if response.is_valid:
        battery = response.battery_state
        print(f"Battery level: {battery.percentage}%")
        print(f"Voltage: {battery.voltage}V")
        print(f"Current: {battery.current}A")
        print(f"Temperature: {battery.temperature}°C")

        # Check charging status
        if battery.power_supply_status == 1:  # CHARGING
            print("Battery is charging")
        elif battery.power_supply_status == 4:  # FULL
            print("Battery is full")
    else:
        print("Battery data is not valid or unavailable")

Note

  • Data from /nav/battery_state topic subscription (5 second timeout)

  • Always check is_valid before using battery_state data

  • When is_valid=false, battery_state will be an empty/default BatteryState message

  • Uses sensor_msgs/BatteryState message type

is_charge()

Check if the robot is currently charging.

This function returns whether the robot is currently charging with automatic data validation. If the charge state data is stale or not available, it returns false.

Returns:

Response containing state and charging status

  • state: State object with code and describe

  • is_charging: Boolean indicating if robot is charging

    • true: Robot is currently charging

    • false: Robot is not charging (or data unavailable)

Return type:

IsChargeResponse

Examples:

# Check if charging
response = is_charge()
if response.state.code == 0:  # StateCode.success
    if response.is_charging:
        print("Robot is charging")
    else:
        print("Robot is not charging")

# Simple usage
if is_charge().is_charging:
    print("Charging in progress")

Note

  • Data from /nav/charge_state topic subscription (5 second timeout)

  • Returns is_charging = false if data is stale or not yet received

  • Uses std_msgs/Bool message type

速度控制

send_cmd_vel(linear_x=0.0, linear_y=0.0, angular_z=0.0, dt=1.0, blocking=True)

Send velocity command to control robot motion via /cmd_vel topic (supports omnidirectional movement).

This function publishes Twist messages to control the robot’s linear velocities (forward/backward and left/right) and angular velocity. It supports both blocking and non-blocking modes.

Parameters:

  • linear_x (float) – Forward/backward linear velocity in m/s - Positive value: move forward - Negative value: move backward - Recommended ranges depend on gait and terrain (see Note below)

  • linear_y (float) – Left/right linear velocity in m/s (omnidirectional) - Positive value: move left - Negative value: move right - Recommended ranges depend on gait and terrain (see Note below)

  • angular_z (float) – Angular velocity in rad/s - Positive value: turn counter-clockwise (left) - Negative value: turn clockwise (right) - Recommended ranges depend on gait and terrain (see Note below)

  • dt (float) – Duration in seconds for the robot to move - Must be non-negative - dt > 0: Robot moves for this duration at 10Hz, then automatically stops - dt = 0: Publishes velocity command once only (no automatic stop)

  • blocking (bool) – Whether to block until motion completes (default: True) - True: Function blocks and waits until dt expires (if dt > 0) - False: Function returns immediately, motion continues in background

Returns:

Response containing state and success flag

  • state.code: StateCode (0=success, 1=fail, etc.)

  • state.describe: Description of the result

  • success: Boolean indicating if command was published successfully

Return type:

SendCmdVelResponse

Examples:

# Stop robot for 1 second (using all defaults: 0, 0, 0, 1.0, True)
result = send_cmd_vel()
if result.success:
    print("Robot stopped for 1 second")

# Move forward for 2 seconds (using keyword args)
result = send_cmd_vel(linear_x=0.2, dt=2.0)
if result.success:
    print("Robot moved forward and stopped")

# Move left (omnidirectional) for 1.5 seconds
result = send_cmd_vel(linear_y=0.3, dt=1.5)
if result.success:
    print("Robot moved left and stopped")

# Rotate in place for default 1 second
result = send_cmd_vel(angular_z=0.5)

# Turn left while moving forward
result = send_cmd_vel(linear_x=0.2, angular_z=0.3, dt=2.0)

# Move diagonally (forward + left)
result = send_cmd_vel(linear_x=0.2, linear_y=0.2, dt=2.0)

# Non-blocking mode - robot moves in background
result = send_cmd_vel(angular_z=0.2, blocking=False)
if result.success:
    print("Robot is turning in background for 1 second")
    # Next line executes immediately, robot continues turning
    do_other_work()  # Robot still turning while this executes

# Publish once without automatic stop (use with caution!)
result = send_cmd_vel(linear_x=0.1, dt=0.0)
if result.success:
    print("Command sent, robot will continue until stopped manually")
    # Must manually send stop command later: send_cmd_vel(dt=0.0) or send_cmd_vel()

Note

Recommended velocity ranges (depends on gait and terrain):

  • Regular gait:

    • Flat ground: linear.x: [-1.0, 2.0] m/s, linear.y: [-0.4, 0.4] m/s, angular.z: [-0.8, 0.8] rad/s

    • Slope: linear.x: [-0.8, 0.8] m/s, linear.y: [-0.4, 0.4] m/s, angular.z: [-0.6, 0.6] rad/s

  • RL gait (all terrains):

    • linear.x: [-2.0, 2.0] m/s, linear.y: [-0.4, 0.4] m/s, angular.z: [-0.8, 0.8] rad/s

Important: - When dt > 0: Robot automatically stops after dt seconds (all axes: x, y, z) - When dt = 0: Robot continues moving until you send stop command (linear_x=0, linear_y=0, angular_z=0) - blocking=True: Python execution waits until motion completes - blocking=False: Python execution continues immediately, motion happens in background - Uses geometry_msgs/Twist message type - Publishes to /cmd_vel topic at 10Hz when dt > 0

相关数据类型

运动控制相关的响应类型和枚举类型的详细说明请参见 数据结构 文档：

响应类型:

枚举类型: