31自由度の最小制御

本ガイドでは、BXI Elf3 の関節を制御する 2 つの方法を紹介します。頭部のみを制御する場合は、コードを書かずに、関節名を指定したコマンドを actuators_cmds_override に直接パブリッシュします。31 自由度すべてを制御する場合は、state.py と mod.yaml の 2 ファイルだけを含む Mod を作成し、on_update() 内で軌道を直接生成します。

本サンプルは Mod API 4.0 を対象としています。

• フレームワークの完全なソースコード（導入ユーザー様に HumansX より提供します）
• Mod API の完全なドキュメント（導入ユーザー様に HumansX より提供します）

以下のコマンドは、フレームワークリポジトリのルートディレクトリで実行してください。

はじめに：実機のデフォルトは 29 自由度です

実機用 launch はデフォルトで enable_head:=auto を使用し、ロボットのモジュール構成を /opt/bxi/robot_config.yaml から読み込みます。頭部の 2 関節を含む 31 自由度の実機構成を有効にするには、次の 2 つの条件を両方とも満たす必要があります。

1. /opt/bxi/robot_config.yaml が存在し、正常に読み込めること
2. 設定に modules.head.enabled: true が含まれていること

最小限の設定構造は以下のとおりです。本番用の設定は bxi_robot_config_tool で生成してください。

modules:
  head:
    enabled: true

次のいずれかに該当する場合は、頭部モーターを含まない組み込みの 29 自由度構成にフォールバックします。

• /opt/bxi/robot_config.yaml が存在しない、読み込めない、または書式が不正である
• modules.head.enabled が記述されていない
• modules.head.enabled が false である

現在の実装では、頭部の有効・無効にかかわらず同じ hardware_elf3 パッケージを使用します。頭部モーターが有効かどうかは、起動ログの motor_disable マスクで確認してください。

# 頭部有効（この例では他のモーター無効化ビットは立っていません）
[bxi hardware config] package=hardware_elf3, motor_disable=0x00000000, ...

# 頭部無効（この例では他のモーター無効化ビットは立っていません）
[bxi hardware config] package=hardware_elf3, motor_disable=0x60000000, ...

他のモーターも無効化されている場合、motor_disable の値全体は上記の例と異なることがあります。頭部のマスクのみを確認してください。motor_disable & 0x60000000 == 0 であれば頭部は有効、motor_disable & 0x60000000 == 0x60000000 であれば無効です。hardware_elf3_head と hardware_elf3 のパッケージ名の違いから自由度モードを判断しないでください。

29 自由度モードでは、実行時の関節レイアウトに head_z_joint と head_y_joint が含まれないため、後述の頭部オーバーライドコマンドは「不明な関節名を含む」として拒否されます。先に有効な /opt/bxi/robot_config.yaml を生成し、実機プログラムを再起動してください。なお、シミュレーション用の Elf3 モデルには最初から頭部の 2 関節が含まれており、この実機用設定の影響は受けません。

1. コード不要の頭部関節オーバーライド

コントローラーは最終出力段で communication/msg/ActuatorCmds 型のオーバーライドコマンドを読み取り、actuators_name に列挙された関節のみを置き換えます。その他の関節は引き続き現在の状態またはポリシーによって制御されるため、頭部のみの制御に Mod や 31 要素すべてのコマンドは必要ありません。

オーバーライド用トピックには、launch 時の topic_prefix が付きます。

ゼロトルクモードでは、オーバーライドはデフォルトで無効です。ロボットを起動して関節レイアウトの初期化を待ったあと、ゼロ位置モードまたは他のアクティブ制御状態に入ってください。たとえば、ゼロ位置モードには次のコマンドで入ります。

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

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

次のコマンドは、頭部の 2 関節を 20 Hz でオーバーライドします。head_z_joint と head_y_joint の目標値は、それぞれ 0.20 rad と -0.10 rad です。

# シミュレーション
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]
  }'

actuators_name は、すべてのコマンド配列における関節の順序を定義します。pos、kp、kd の長さは関節名の数と一致している必要があります。省略した vel と torque フィールドは 0 として扱われます。

オーバーライドのデフォルトのタイムアウトは 0.2 s のため、--once では目標値を保持できません。20 Hz のパブリッシャーを停止するとコマンドは自動的に失効し、0.2 s かけて現在の状態の出力へ滑らかに戻ります。空のメッセージを送ると、明示的に解除を要求できます。

# シミュレーション（実機では simulation を hardware に置き換えてください）
ros2 topic pub --once \
  /simulation/actuators_cmds_override \
  communication/msg/ActuatorCmds '{}'

時間とともに変化する頭部軌道を生成するには、ROS ノードから同じ型のメッセージを継続的にパブリッシュします。新しいメッセージを受信するたびに、直前のオーバーライドがアトミックに置き換えられます。ステートマシンとの統合、全身の軌道、またはより複雑な制御ロジックが必要な場合は、次に説明する Mod 方式を使用してください。

2. 31 関節の順序

MotorFrame 内の qpos、kp、kd 配列は、同じ関節レイアウトを使用する必要があります。本サンプルの数式配列は以下の順序を使用します。位置の単位はラジアンです。

本サンプルでは、このレイアウトを MotorFrame に明示的に関連付けています。実行時のレイアウトは、同じ名前の 31 関節を含んでいれば異なる順序でも構いません。フレームワークが関節名に基づいてコマンドを実際の順序へマッピングします。

3. Mod の作成

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

必要なファイルは次の 2 つだけです。

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

4. 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 要素はゼロ位置モードの
    # ゲイン、最後の 2 要素は頭部のゲインです。
    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 joint set mismatch: "
                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

重要なポイントは次の 4 点です。

1. qpos、KP、KD はいずれも ELF3_LAYOUT に基づいて解釈される 31 要素の float32 配列です。そのため、この状態は毎サイクル、ロボットのレイアウト全体に指令を出します。
2. phase は、ループの実行回数を前提とせず、フレームワークから渡される dt に基づいて進みます。
3. _motor_frame() は状態内部の MotorFrame バッファを再利用し、_apply_frame() がそれをフレームワークに送信します。
4. EntryFrameProvider と RunningFrameProvider はどちらも実装しておらず、軌道は on_update() のみで生成されます。

現在の MotorFrame は MIT 制御の vel および torque フィールドにも対応しています。本サンプルでは _motor_frame() 呼び出し時に両方を省略しているため、フレームワークが明示的に 0 を設定します。

この数式で動くのは 6 関節のみですが、残りの 25 関節も qpos=0 と対応する kp/kd によって引き続きアクティブに制御されています。動作を変更する場合は、配列の順序は変えずに、on_update() 内の該当インデックスの数式を編集してください。

5. mod.yaml の作成

schema: 1
id: com.example.elf3_31dof
name: ELF3 31-DoF Formula Trajectory
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-DoF Formula Trajectory
    priority: 100
    group: Customer
    icon: waves
    confirm: true
    confirm_message: This unbalanced example is for simulation only

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

2 つのルートでは、意図的に transition を省略しています。プロジェクトの elf3_state_machine.yaml では default_transition が instant に設定されているため、状態は即座に切り替わり、遷移先の状態の on_update() は次の制御サイクルから実行されます。本サンプルにおける「遷移なし」とはこの意味です。システムのデフォルトが変更されている場合は、両方のルートに transition: instant を明示的に追加してください。

本サンプルには com.bxi.basic_actions/initial_pos（ゼロ位置モード）からのみ入ることができます。これは即時切り替え時の目標値の不連続を小さくするとともに、数式軌道にバランス維持機能がないことを反映したものです。normal、歩行、その他の動作状態からのルートは追加しないでください。入る際は btn_10=99 を送信し、戻る際はゼロ位置モードのコマンドを使用します。btn_10=10 はすでに com.bxi.any_motion/activate に割り当てられているため、本サンプルでは使用しません。

6. ビルドと実行

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}"

ロボットがゼロ位置モードに入ったら、btn_10=99 を送信して数式軌道の状態に入ります。

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}"

btn_10=0 のパブリッシュはボタンを離した状態に戻すだけで、入ったばかりの数式軌道状態から抜けるものではありません。

正常に読み込まれると、次のログが出力されます。

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

完全修飾された状態名は com.example.elf3_31dof/formula_31dof です。

トラブルシューティング

関節レイアウトが一致しない

実際の ctx.robot_layout.names を確認してください。フレームワークが ELF3_LAYOUT の関節名を自動的にマッピングするため、順序が異なっていても制御には影響しません。レイアウトと 3 つの 31 要素配列を更新する必要があるのは、関節が不足している場合、またはハードウェアが異なる関節名を使用している場合のみです。

状態は読み込まれるがロボットが動かない

source install/setup.bash で最新のワークスペースが読み込まれていることを確認し、起動ログに com.example.elf3_31dof が含まれているかを確認してください。シンボリックリンクを使用しないインストールでは、Mod を変更するたびに再ビルドが必要です。

切り替え時にロボットが急に動く

これは即時切り替えで想定されるリスクです。先にゼロ位置モードに入り、elapsed=0 のときに on_update() がゼロ姿勢を出力するようにしてください。任意の姿勢から滑らかに状態へ入る必要がある場合は Transition を使用してください。これは本最小サンプルの範囲外です。