最简控制 ELF3 的 31 自由度

modules:
  head:
    enabled: true

# 31 自由度
[bxi hardware config] package=hardware_elf3_head, ...

# 默认 29 自由度
[bxi hardware config] package=hardware_elf3, ...

ros2 topic pub --once /motion_commands \
  communication/msg/MotionCommands "{btn_4: 1}"

ros2 topic pub --once /motion_commands \
  communication/msg/MotionCommands "{btn_4: 0}"

# 仿真
ros2 topic pub -r 20 \
  /simulation/actuators_cmds_override \
  communication/msg/ActuatorCmds \
  '{
    actuators_name: ["head_z_joint", "head_y_joint"],
    pos: [0.20, -0.10],
    kp: [16.747, 16.747],
    kd: [1.066, 1.066]
  }'

# 真机；执行前必须完成吊装并准备急停
ros2 topic pub -r 20 \
  /hardware/actuators_cmds_override \
  communication/msg/ActuatorCmds \
  '{
    actuators_name: ["head_z_joint", "head_y_joint"],
    pos: [0.20, -0.10],
    kp: [16.747, 16.747],
    kd: [1.066, 1.066]
  }'

# 仿真；真机时将 simulation 替换为 hardware
ros2 topic pub --once \
  /simulation/actuators_cmds_override \
  communication/msg/ActuatorCmds '{}'

cd ~/bxi_ws/bxi_rl_controller_ros2_example
mkdir -p src/bxi_example_py_elf3/mods/com.example.elf3_31dof

src/bxi_example_py_elf3/mods/com.example.elf3_31dof/
├── mod.yaml
└── state.py

import math

import numpy as np

from bxi_example_py_elf3.framework.mod_api import JointLayout, RobotControlState


ELF3_JOINT_NAMES = (
    "waist_y_joint", "waist_x_joint", "waist_z_joint",
    "l_hip_y_joint", "l_hip_x_joint", "l_hip_z_joint",
    "l_knee_y_joint", "l_ankle_y_joint", "l_ankle_x_joint",
    "r_hip_y_joint", "r_hip_x_joint", "r_hip_z_joint",
    "r_knee_y_joint", "r_ankle_y_joint", "r_ankle_x_joint",
    "l_shoulder_y_joint", "l_shoulder_x_joint", "l_shoulder_z_joint",
    "l_elbow_y_joint", "l_wrist_x_joint", "l_wrist_y_joint",
    "l_wrist_z_joint", "r_shoulder_y_joint", "r_shoulder_x_joint",
    "r_shoulder_z_joint", "r_elbow_y_joint", "r_wrist_x_joint",
    "r_wrist_y_joint", "r_wrist_z_joint", "head_z_joint",
    "head_y_joint",
)
ELF3_LAYOUT = JointLayout(ELF3_JOINT_NAMES, label="ELF3 31-DoF")


class Elf31DofState(RobotControlState):
    # 顺序与 ELF3_JOINT_NAMES 完全一致。前 29 项沿用零位模式增益，
    # 最后两项是头部关节增益。
    KP = np.array([
        500, 500, 300,
        300, 100, 100, 300, 50, 50,
        300, 100, 100, 300, 50, 50,
        100, 80, 80, 100, 20, 20, 20,
        100, 80, 80, 100, 20, 20, 20,
        16.747, 16.747,
    ], dtype=np.float32)
    KD = np.array([
        3, 3, 3,
        2.5, 2, 2, 2.5, 2, 2,
        2.5, 2, 2, 2.5, 2, 2,
        2.5, 2, 2, 2.5, 1, 1, 1,
        2.5, 2, 2, 2.5, 1, 1, 1,
        1.066, 1.066,
    ], dtype=np.float32)

    def __init__(self, name, state_id):
        super().__init__(name, state_id)
        self.elapsed = 0.0
        self.qpos = np.zeros(31, dtype=np.float32)

    def on_enter(self, ctx):
        if frozenset(ctx.robot_layout.names) != frozenset(ELF3_JOINT_NAMES):
            raise RuntimeError(
                "ELF3 关节集合不匹配: "
                f"expected={ELF3_JOINT_NAMES}, actual={ctx.robot_layout.names}"
            )
        self.elapsed = 0.0

    def on_update(self, ctx, dt):
        # 每周期从零位重新生成目标，避免把偏移逐帧累加。
        self.qpos.fill(0.0)
        phase = 2.0 * math.pi * 0.25 * self.elapsed

        # 写死的 31 自由度示例轨迹：腰腿保持零位，双臂和头部小幅摆动。
        self.qpos[15] = 0.15 * math.sin(phase)        # 左肩 y
        self.qpos[18] = 0.12 * math.sin(phase)        # 左肘 y
        self.qpos[22] = -0.15 * math.sin(phase)       # 右肩 y
        self.qpos[25] = -0.12 * math.sin(phase)       # 右肘 y
        self.qpos[29] = 0.10 * math.sin(phase)        # 头部 z
        self.qpos[30] = 0.06 * math.sin(2.0 * phase)  # 头部 y

        frame = self._motor_frame(
            ctx, self.qpos, self.KP, self.KD, layout=ELF3_LAYOUT
        )
        self._apply_frame(ctx, frame)
        self.elapsed += dt

schema: 1
id: com.example.elf3_31dof
name: ELF3 31 自由度公式轨迹
version: 1.0.0
api: ">=4,<5"
enable: true
entrypoint: null
visibility: public
requires:
  - id: com.bxi.basic_actions
    version: ">=1,<2"
conflicts: []
python_exports: []
runtime_requirements:
  python: []
  ros: []
  system: []

events:
  activate:
    slot: btn_10
    value: 99

states:
  formula_31dof:
    factory: state:Elf31DofState
    label: 31 自由度公式轨迹
    priority: 100
    group: Customer
    icon: waves
    confirm: true
    confirm_message: 此示例没有平衡能力，仅限仿真运行

routes:
  - from: com.bxi.basic_actions/initial_pos
    event: activate
    to: formula_31dof

  - from: formula_31dof
    event: com.bxi.basic_actions/initial_pos
    to: com.bxi.basic_actions/initial_pos

colcon build --packages-select bxi_example_py_elf3 \
  --symlink-install --merge-install
source install/setup.bash

ros2 launch bxi_example_py_elf3 example_demo.launch.py

ros2 topic pub --once /motion_commands \
  communication/msg/MotionCommands "{btn_4: 1}"

ros2 topic pub --once /motion_commands \
  communication/msg/MotionCommands "{btn_4: 0}"

ros2 topic pub --once /motion_commands \
  communication/msg/MotionCommands "{btn_10: 99}"

# 必须释放 btn_10，否则后续 btn_10 动作可能无法触发
ros2 topic pub --once /motion_commands \
  communication/msg/MotionCommands "{btn_10: 0}"

[com.example.elf3_31dof]: loaded v1.0.0: .../com.example.elf3_31dof