人形ロボット 3D（物理エンジン駆動）
3D は物理シミュの /joint_states(関節) と /model_pose(ベース) で動きます。重力で落下・接地し、姿勢を崩すと転倒します。
📋 この操作をコードで (topic 購読)
# コンテナ内で実行: docker exec -it ros2-lab bash
source /opt/ros/jazzy/setup.bash
ros2 topic echo /joint_states --once # 関節角を 1 回受信
ros2 topic hz /joint_states # 配信レート(≈30Hz)
# rclpy (Python)
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
rclpy.init()
node = Node("listener")
node.create_subscription(JointState, "/joint_states",
lambda m: print(dict(zip(m.name, m.position))), 10)
rclpy.spin(node)目標姿勢（位置制御）
📋 この操作をコードで (topic 発行)
# 両手を上げる (discovery のため数回 publish: -r 5 -t 5)
ros2 topic pub -r 5 -t 5 /target_joint_positions sensor_msgs/msg/JointState \
"{name: [l_shoulder, r_shoulder], position: [3.0, 3.0]}"
# rclpy (Python)
pub = node.create_publisher(JointState, "/target_joint_positions", 10)
msg = JointState(name=["l_shoulder", "r_shoulder"], position=[3.0, 3.0])
pub.publish(msg) # 実運用は timer で数回 publish すると確実物理シミュ状態 (PyBullet)
目標角 → /target_joint_positions → PyBullet(重力 -9.81, 接地) → /joint_states + /model_pose → 3D
バランス制御(足首戦略 PD)で直立を維持。「押す（外乱）」で前方に押すと、 ON なら踏ん張って復帰、OFF なら倒れます。⤓ で起き上がり。
⚠️ 歩行・横方向バランスは未実装です。足首戦略だけでは重心が足裏を越える外乱を戻せず、踏み出し(capture point)や全身 MPC という別クラスの制御が必要なため — これ自体がバランス制御の重要な教材です。📖 詳しく
📋 この操作をコードで (service 呼び出し)
ros2 service call /set_balance std_srvs/srv/SetBool "{data: false}" # バランス OFF
ros2 service call /push std_srvs/srv/Trigger # 前方 140N の外乱
ros2 service call /reset_sim std_srvs/srv/Trigger # 起き上がり/リセット⚖ 接地力 / ZMP バランスの実測 (Phase 14)
足裏の接地力(法線力)と ZMP(圧力中心)。ZMP が足裏(支持多角形)の中にある間は立っていられ、端に達すると転倒します。押す(外乱)と赤点が前に動くのを観察してください。
📋 この操作をコードで (力センサ/ZMP topic)
ros2 topic echo /zmp --once # ZMP(圧力中心, 世界座標) ros2 topic echo /foot_force_left --once # 左足の法線力 (geometry_msgs/WrenchStamped) ros2 topic echo /com --once # 全身重心の床投影 ros2 topic hz /zmp # ≈30Hz
ロボット視点カメラ 首スライダで左右パン
頭部カメラの一人称視点(JPEG 10Hz)。1 回のレンダから RGB / 深度 / セグメンテーションの 3 ストリームを配信 — 実機の RGB-D カメラと同じ構成です。色柱(赤=正面/緑=左前/青=右前/黄=背面)。
📋 この操作をコードで (画像 topic)
ros2 topic hz /camera/image_raw/compressed # ≈10Hz
ros2 topic echo /camera/image_raw/compressed --once --no-arr # ヘッダのみ表示
# 首を回してカメラをパン (neck はヨー軸)
ros2 topic pub -r 5 -t 5 /target_joint_positions sensor_msgs/msg/JointState \
"{name: [neck], position: [1.2]}"
# 深度 / セグメンテーション
ros2 topic hz /camera/depth/compressed
ros2 topic hz /camera/segmentation/compressed🌀 LiDAR (2D スキャン) /scan (sensor_msgs/LaserScan, 5Hz)
base 高さ 0.35m の水平 360° レーザースキャン(180 本、機体固定)。周囲の色柱が点列として見えます。上=前方、中心=ロボット。自律移動の目になるセンサです。
📋 この操作をコードで (LaserScan)
ros2 topic hz /scan # ≈5Hz ros2 topic echo /scan --once --no-arr # メタ情報(角度範囲・分解能) # rclpy: msg.ranges[i] が角度 angle_min + i*angle_increment の距離[m]
🌳 tf ツリー /tf をライブ購読 (v1.1)
全フレームの親子関係と変換をライブ表示。robot_state_publisher が URDF+関節角から、physics_sim が world→base_link を配信しています。関節を動かすと値が変わります。
(受信待ち…)
フレームを選択…
📋 この操作をコードで (tf)
ros2 run tf2_ros tf2_echo base_link head # 2 フレーム間の変換をライブ表示 ros2 run tf2_tools view_frames # ツリーを PDF に出力 ros2 topic echo /tf --once # 生の TransformStamped 配列
📶 QoS 実験 durability / history depth (v1.1)
QoS は DDS 通信の性質を決める設定。決定的に再現する実験で体感できます(実験はノード内で完結し、結果を service で受け取ります)。
boot 時に 1 回だけ送られたメッセージを「後から購読」して届くか試します。
📋 この操作をコードで (QoS)
# CLI で latch を体感(後から購読しても届く) ros2 topic echo /qos/latched --qos-durability transient_local --once ros2 topic echo /qos/volatile --once # ← こちらは何も来ない # rclpy で QoS 指定 from rclpy.qos import QoSProfile, DurabilityPolicy qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) node.create_publisher(String, "/qos/latched", qos)
♻ lifecycle ノード managed node の状態機械 (v1.1)
lifecycle ノードは状態機械で管理される: /lifecycle/heartbeat は active のときだけ流れます(inactive では publish がドロップ)。遷移させて確かめてください。
unconfigured ──configure──▶ inactive ──activate──▶ active
▲ cleanup ──┘ ▲── deactivate ──┘
📋 この操作をコードで (lifecycle)
ros2 lifecycle get /lifecycle_demo # 現在の状態 ros2 lifecycle set /lifecycle_demo configure # unconfigured → inactive ros2 lifecycle set /lifecycle_demo activate # inactive → active ros2 topic echo /lifecycle/heartbeat # active のときだけ流れる
🦊 Foxglove Studio 連携 業界標準ツール (v1.1)
このラボの全 topic / tf / カメラ画像は、ロボティクス業界標準の可視化ツール Foxglove Studio からも見られます(foxglove_bridge が ws :16552 で待機)。自作 UI(rosbridge) と業界標準の両対応です。
1. Foxglove Studio (デスクトップ版) を起動 2. 「Open connection」→ Foxglove WebSocket 3. URL: ws://192.168.11.90:16552 (LAN 内) 4. 3D パネルに /tf・Image パネルに /camera/image_raw/compressed 等を追加
※ Web 版 (app.foxglove.dev) は https のため ws:// に接続できません — デスクトップ版を使うか、wss 中継が必要です。
🎯 ROS2 Action デモ /perform_motion (control_msgs/FollowJointTrajectory)
ROS2 の第3の通信「アクション」= 長時間ゴール + 進捗フィードバック + キャンセル可。動作を選ぶと軌道ゴールを送り、physics_sim がバランス下で実行します(topic/service との違いを体感)。
📋 この操作をコードで (action)
ros2 action list # /perform_motion が見える
ros2 action send_goal --feedback /perform_motion \
control_msgs/action/FollowJointTrajectory \
"{trajectory: {joint_names: [l_shoulder, r_shoulder],
points: [{positions: [3.0, 3.0], time_from_start: {sec: 2}}]}}"
# rclpy (Python)
from rclpy.action import ActionClient
from control_msgs.action import FollowJointTrajectory
ac = ActionClient(node, FollowJointTrajectory, "/perform_motion")
ac.wait_for_server()
goal = FollowJointTrajectory.Goal()
goal.trajectory.joint_names = ["l_shoulder", "r_shoulder"]
# goal.trajectory.points に JointTrajectoryPoint(positions, time_from_start) を追加
future = ac.send_goal_async(goal) # 中断は goal_handle.cancel_goal_async()🦾 腕リーチ (IK) 2 リンク逆運動学 → action 実行 (v1.1)
肩フレームの目標 (前後 x, 上下 z) を決めると、バックエンドが閉形式 2 リンク IK で肩・肘の角度を解き、/perform_motion action で腕を伸ばします。3D の橙のボールが目標 — 届くと緑になります(tf で手先位置を検証)。腕は肩の縦平面しか動けない(肩・肘とも同軸ピッチ)ことも体感できます。
📋 この操作をコードで (2リンク IK)
# IK を解く(バックエンド API)
curl "https://ros2-lab.akylabo.com/api/arm-ik?x=0.35&z=0.0"
# → {"shoulder": 0.79, "elbow": -1.62, ...}
# 閉形式 2 リンク IK (L1=0.26, L2=0.24):
# cos(q2) = (x²+z² − L1² − L2²) / (2·L1·L2) # 肘の曲げ量
# shoulder = atan2(x, −z) − atan2(L2·sin(q2), L1 + L2·cos(q2))
# elbow = −q2 # URDF は負方向が曲げ
# 解いた角度を action で実行
ros2 action send_goal /perform_motion control_msgs/action/FollowJointTrajectory \
"{trajectory: {joint_names: [r_shoulder, r_elbow],
points: [{positions: [0.79, -1.62], time_from_start: {sec: 2}}]}}"🛩 フライトレコーダ / ブラックボックス
テレメトリ(高さ z / 傾き / バランス)と事象(押す・転倒・復帰・バランス切替・リセット)を SQLite に記録(/api/recorder/*)。エピソード(⤓リセット間)を選ぶと、そこで何が起きたかを「ブラックボックス」として振り返れます。
サービス呼び出し — /add_two_ints
a
b
📋 この操作をコードで (service)
ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 2, b: 3}"
# rclpy (Python)
from example_interfaces.srv import AddTwoInts
cli = node.create_client(AddTwoInts, "/add_two_ints")
cli.wait_for_service()
fut = cli.call_async(AddTwoInts.Request(a=2, b=3))
rclpy.spin_until_future_complete(node, fut)
print(fut.result().sum) # -> 5