# ROS 2 (Robot Operating System) によるロボットノードの基本
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
import numpy as np
 
class ObstacleAvoidanceNode(Node):
    def __init__(self):
        super().__init__('obstacle_avoidance')
 
        # LiDARデータの購読
        self.scan_sub = self.create_subscription(
            LaserScan, '/scan', self.scan_callback, 10
        )
 
        # 速度指令の発行
        self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)
 
        self.min_distance = 0.5  # 最小安全距離 (メートル)
        self.get_logger().info('障害物回避ノード起動')
 
    def scan_callback(self, msg: LaserScan):
        """LiDARスキャンデータから障害物を検出し回避"""
        ranges = np.array(msg.ranges)
        ranges = np.where(np.isinf(ranges), 10.0, ranges)
 
        # 前方180度のスキャンデータ
        front_ranges = ranges[len(ranges)//4 : 3*len(ranges)//4]
 
        cmd = Twist()
 
        if np.min(front_ranges) < self.min_distance:
            # 障害物検出 → 回転
            cmd.angular.z = 0.5  # 左旋回
            cmd.linear.x = 0.0
            self.get_logger().warn(
                f'障害物検出: {np.min(front_ranges):.2f}m — 回避中'
            )
        else:
            # 安全 → 前進
            cmd.linear.x = 0.3
            cmd.angular.z = 0.0
 
        self.cmd_pub.publish(cmd)
 
def main():
    rclpy.init()
    node = ObstacleAvoidanceNode()
    rclpy.spin(node)
    rclpy.shutdown()

import torch
import torch.nn as nn
from torch.utils.data import DataLoader
 
class RobotPolicy(nn.Module):
    """画像観測から行動を予測する方策ネットワーク"""
    def __init__(self, action_dim=7):
        super().__init__()
        # 視覚エンコーダ
        self.vision = nn.Sequential(
            nn.Conv2d(3, 32, 3, stride=2, padding=1),
            nn.ReLU(),
            nn.Conv2d(32, 64, 3, stride=2, padding=1),
            nn.ReLU(),
            nn.AdaptiveAvgPool2d(4),
            nn.Flatten(),
        )
        # 行動予測ヘッド
        self.policy = nn.Sequential(
            nn.Linear(64 * 16, 256),
            nn.ReLU(),
            nn.Linear(256, 128),
            nn.ReLU(),
            nn.Linear(128, action_dim),  # [dx, dy, dz, droll, dpitch, dyaw, gripper]
        )
 
    def forward(self, image):
        features = self.vision(image)
        action = self.policy(features)
        return action
 
# 人間のデモンストレーションデータから学習
class DemonstrationDataset(torch.utils.data.Dataset):
    def __init__(self, demo_path):
        self.demos = load_demonstrations(demo_path)
        # demo: [(image, action), (image, action), ...]
 
    def __len__(self):
        return len(self.demos)
 
    def __getitem__(self, idx):
        image, action = self.demos[idx]
        return torch.tensor(image).float(), torch.tensor(action).float()
 
# 学習ループ
policy = RobotPolicy().to(device)
optimizer = torch.optim.Adam(policy.parameters(), lr=1e-4)
 
for epoch in range(100):
    for images, expert_actions in dataloader:
        predicted_actions = policy(images.to(device))
        loss = nn.MSELoss()(predicted_actions, expert_actions.to(device))
        optimizer.zero_grad()
        loss.backward()
        optimizer.step()

import torch
import torch.nn as nn
 
class DiffusionPolicy(nn.Module):
    """
    Diffusion Policy: 拡散モデルを使ったロボット行動生成
    ノイズから行動軌道を段階的にデノイズして生成する
 
    利点:
    - マルチモーダルな行動分布を表現可能
    - 複雑な操作タスクで高い成功率
    - 2024-2025年のロボティクスで最も注目される手法
    """
    def __init__(self, obs_dim=512, action_dim=7, action_horizon=16,
                 n_diffusion_steps=100):
        super().__init__()
        self.action_dim = action_dim
        self.action_horizon = action_horizon
        self.n_steps = n_diffusion_steps
 
        # 観測エンコーダ（画像→特徴量）
        self.obs_encoder = nn.Sequential(
            nn.Conv2d(3, 64, 3, stride=2, padding=1),
            nn.ReLU(),
            nn.Conv2d(64, 128, 3, stride=2, padding=1),
            nn.ReLU(),
            nn.AdaptiveAvgPool2d(4),
            nn.Flatten(),
            nn.Linear(128 * 16, obs_dim),
        )
 
        # ノイズ予測ネットワーク（1D U-Net的構造）
        self.noise_pred_net = nn.Sequential(
            nn.Linear(action_dim * action_horizon + obs_dim + 1, 512),
            nn.ReLU(),
            nn.Linear(512, 512),
            nn.ReLU(),
            nn.Linear(512, action_dim * action_horizon),
        )
 
    def forward(self, obs, noisy_action, timestep):
        """ノイズ予測: 現在の観測と汚染された行動からノイズを推定"""
        obs_feat = self.obs_encoder(obs)
        noisy_flat = noisy_action.flatten(start_dim=1)
        t_embed = timestep.float().unsqueeze(1) / self.n_steps
 
        x = torch.cat([noisy_flat, obs_feat, t_embed], dim=1)
        noise_pred = self.noise_pred_net(x)
        return noise_pred.view(-1, self.action_horizon, self.action_dim)
 
    @torch.no_grad()
    def generate_action(self, obs):
        """推論: ノイズから行動軌道を段階的にデノイズ"""
        batch_size = obs.shape[0]
        device = obs.device
 
        # ランダムノイズから開始
        action = torch.randn(
            batch_size, self.action_horizon, self.action_dim, device=device
        )
 
        # DDPM デノイジングプロセス
        for t in reversed(range(self.n_steps)):
            timestep = torch.full((batch_size,), t, device=device)
            noise_pred = self.forward(obs, action, timestep)
 
            # デノイジングステップ（簡略化）
            alpha = 1 - 0.02 * t / self.n_steps
            action = (action - (1 - alpha) * noise_pred) / alpha.sqrt()
 
            if t > 0:
                action += 0.1 * torch.randn_like(action)
 
        return action  # (batch, horizon, action_dim)
 
# 学習
policy = DiffusionPolicy().to(device)
optimizer = torch.optim.AdamW(policy.parameters(), lr=1e-4)
 
for epoch in range(100):
    for obs, expert_actions in dataloader:
        # ランダムタイムステップでノイズ追加
        t = torch.randint(0, policy.n_steps, (obs.shape[0],), device=device)
        noise = torch.randn_like(expert_actions)
        noisy_actions = expert_actions + noise * (t.float() / policy.n_steps).unsqueeze(1).unsqueeze(2)
 
        # ノイズ予測の学習
        noise_pred = policy(obs.to(device), noisy_actions.to(device), t)
        loss = nn.MSELoss()(noise_pred, noise.to(device))
 
        optimizer.zero_grad()
        loss.backward()
        optimizer.step()

# VLM（Vision-Language Model）によるロボット制御の概念コード
class VLMRobotController:
    """
    言語指示を理解し、カメラ画像から状況判断して
    ロボットの行動を生成するコントローラ
    """
    def __init__(self):
        self.vlm = load_vlm("rt-2-x")  # Robotics Transformer
        self.low_level_controller = MotorController()
 
    def execute_instruction(self, instruction: str, camera_image):
        """
        例: instruction = "テーブルの上の赤いカップを取って、棚に置いて"
        """
        # VLMが画像と指示を理解し、行動トークンを生成
        action_tokens = self.vlm.predict(
            image=camera_image,
            instruction=instruction,
        )
        # action_tokens → [dx, dy, dz, rx, ry, rz, gripper_open]
 
        # 低レベル制御器でモーター指令に変換
        for action in action_tokens:
            joint_torques = self.low_level_controller.inverse_kinematics(action)
            self.low_level_controller.execute(joint_torques)
 
        return action_tokens
 
# Google RT-2-X のアプローチ:
# 1. 大規模言語モデル（PaLM-E）で言語理解
# 2. ビジョンエンコーダで環境認識
# 3. 行動トークンとして離散化した動作を生成
# 4. デモリファイニング(学習済み)モデルで実行

# NVIDIA Isaac Gym を使った四足歩行ロボットの強化学習
import isaacgym
from isaacgym import gymapi, gymutil
import torch
 
class QuadrupedEnv:
    """四足歩行ロボットの並列シミュレーション環境"""
 
    def __init__(self, num_envs=4096):
        self.num_envs = num_envs
        self.gym = gymapi.acquire_gym()
 
        # シミュレーション設定
        sim_params = gymapi.SimParams()
        sim_params.dt = 1.0 / 200.0  # 200Hz
        sim_params.substeps = 2
        sim_params.physx.solver_type = 1
        sim_params.physx.num_position_iterations = 4
        sim_params.physx.num_velocity_iterations = 0
        sim_params.physx.use_gpu = True
 
        self.sim = self.gym.create_sim(0, 0, gymapi.SIM_PHYSX, sim_params)
 
        # 4096個の環境を並列作成（GPU上で同時シミュレーション）
        self._create_envs()
 
    def _create_envs(self):
        """並列環境の作成"""
        asset_root = "/path/to/urdf/"
        asset_file = "a1_robot.urdf"
 
        # ロボットモデルのロード
        asset = self.gym.load_asset(self.sim, asset_root, asset_file)
 
        for i in range(self.num_envs):
            env = self.gym.create_env(self.sim, ...)
            self.gym.create_actor(env, asset, ...)
 
    def step(self, actions):
        """1ステップ実行（全環境を同時に進める）"""
        # actions: (num_envs, 12) — 各脚3関節 × 4脚
        self.gym.set_dof_position_target_tensor(self.sim, actions)
        self.gym.simulate(self.sim)
        self.gym.fetch_results(self.sim, True)
 
        obs = self._compute_observations()
        rewards = self._compute_rewards()
        dones = self._check_termination()
 
        return obs, rewards, dones
 
    def _compute_rewards(self):
        """報酬関数"""
        # 前進速度への報酬
        forward_reward = self.base_velocity[:, 0] * 2.0
 
        # エネルギーペナルティ（省エネ歩行を促進）
        energy_penalty = -0.005 * torch.sum(self.torques ** 2, dim=1)
 
        # 姿勢安定性（転倒ペナルティ）
        orientation_penalty = -1.0 * torch.sum(
            (self.base_orientation[:, :2]) ** 2, dim=1
        )
 
        # 足の接地パターン（対角歩行を促進）
        gait_reward = self._compute_gait_reward()
 
        return forward_reward + energy_penalty + orientation_penalty + gait_reward
 
# PPO (Proximal Policy Optimization) で学習
# 4096環境を並列 → 1時間で数億ステップの学習が可能
# → 実機では歩行を即座に実行できる

import numpy as np
from dataclasses import dataclass
from typing import List
import h5py
 
@dataclass
class DemoStep:
    """デモの1ステップ"""
    timestamp: float
    image: np.ndarray       # (H, W, 3) RGB画像
    depth: np.ndarray       # (H, W) 深度画像
    joint_positions: np.ndarray   # 関節角度
    joint_velocities: np.ndarray  # 関節速度
    ee_position: np.ndarray      # エンドエフェクタ位置 [x, y, z]
    ee_orientation: np.ndarray   # エンドエフェクタ姿勢 [qx, qy, qz, qw]
    gripper_state: float         # グリッパー開閉 (0-1)
 
class TeleopDataCollector:
    """テレオペレーションによるデモンストレーションデータ収集"""
 
    def __init__(self, robot, camera, save_dir="demos"):
        self.robot = robot
        self.camera = camera
        self.save_dir = save_dir
        self.current_demo: List[DemoStep] = []
        self.demo_count = 0
 
    def start_recording(self):
        """デモ記録開始"""
        self.current_demo = []
        print("デモ記録を開始しました。")
 
    def record_step(self):
        """現在のロボット状態を記録"""
        step = DemoStep(
            timestamp=time.time(),
            image=self.camera.get_rgb(),
            depth=self.camera.get_depth(),
            joint_positions=self.robot.get_joint_positions(),
            joint_velocities=self.robot.get_joint_velocities(),
            ee_position=self.robot.get_ee_position(),
            ee_orientation=self.robot.get_ee_orientation(),
            gripper_state=self.robot.get_gripper_state(),
        )
        self.current_demo.append(step)
 
    def save_demo(self, task_name: str):
        """デモデータをHDF5形式で保存"""
        filename = f"{self.save_dir}/{task_name}_demo_{self.demo_count:04d}.hdf5"
 
        with h5py.File(filename, 'w') as f:
            n_steps = len(self.current_demo)
            f.attrs['task'] = task_name
            f.attrs['n_steps'] = n_steps
 
            # 各データフィールドをバッチで保存
            images = np.stack([s.image for s in self.current_demo])
            f.create_dataset('images', data=images, compression='gzip')
 
            actions = np.stack([
                np.concatenate([s.ee_position, s.ee_orientation, [s.gripper_state]])
                for s in self.current_demo
            ])
            f.create_dataset('actions', data=actions)
 
            joint_pos = np.stack([s.joint_positions for s in self.current_demo])
            f.create_dataset('joint_positions', data=joint_pos)
 
        self.demo_count += 1
        print(f"デモ保存完了: {filename} ({n_steps} ステップ)")
 
# テレオペデバイス
# - VR コントローラー (Meta Quest): 手の位置・姿勢を直接マッピング
# - 3Dマウス (SpaceMouse): 6DoFの入力デバイス
# - リーダーフォロワー: 2台のロボットアームで操作
# - Apple Vision Pro: 手のトラッキングでロボット制御

import numpy as np
from enum import Enum
 
class CoverageState(Enum):
    SPIRAL = "spiral"
    WALL_FOLLOW = "wall_follow"
    RANDOM = "random"
 
class CoveragePlanner:
    """家庭用掃除ロボットのカバレッジ計画"""
 
    def __init__(self, grid_size=(100, 100)):
        self.grid = np.zeros(grid_size, dtype=bool)  # 掃除済みマップ
        self.obstacles = np.zeros(grid_size, dtype=bool)
        self.position = (50, 50)
        self.heading = 0  # 0-359度
        self.state = CoverageState.SPIRAL
 
    def plan_next_action(self, bumper_hit, cliff_detected):
        """センサー入力に基づいて次の行動を決定"""
        if cliff_detected:
            return self._backup_and_turn(180)
 
        if bumper_hit:
            self.state = CoverageState.WALL_FOLLOW
            return self._wall_follow()
 
        if self.state == CoverageState.SPIRAL:
            return self._spiral_outward()
 
        if self._coverage_percentage() > 0.9:
            return self._move_to_uncovered()
 
        return self._random_bounce()
 
    def _coverage_percentage(self):
        """掃除済み面積の割合"""
        cleanable = ~self.obstacles
        return np.sum(self.grid & cleanable) / np.sum(cleanable)
 
    def _spiral_outward(self):
        """スパイラル移動パターン"""
        # 中心から外側に向かって渦巻き状に移動
        # 障害物に当たったら状態遷移
        pass
 
    def _wall_follow(self):
        """壁沿い移動（部屋の端を掃除）"""
        pass
 
    def _move_to_uncovered(self):
        """未掃除エリアへ移動"""
        uncovered = ~self.grid & ~self.obstacles
        if np.any(uncovered):
            target = find_nearest_uncovered(self.position, uncovered)
            return plan_path_to(self.position, target)

import numpy as np
 
class SimpleRobotArm:
    """2リンクロボットアームの逆運動学"""
 
    def __init__(self, l1=0.3, l2=0.25):
        self.l1 = l1  # 第1リンク長 (m)
        self.l2 = l2  # 第2リンク長 (m)
 
    def forward_kinematics(self, theta1, theta2):
        """順運動学: 関節角度 → エンドエフェクタ位置"""
        x = self.l1 * np.cos(theta1) + self.l2 * np.cos(theta1 + theta2)
        y = self.l1 * np.sin(theta1) + self.l2 * np.sin(theta1 + theta2)
        return x, y
 
    def inverse_kinematics(self, x, y):
        """逆運動学: 目標位置 → 関節角度"""
        d = (x**2 + y**2 - self.l1**2 - self.l2**2) / (2 * self.l1 * self.l2)
 
        if abs(d) > 1:
            raise ValueError("目標位置がワークスペース外です")
 
        theta2 = np.arctan2(np.sqrt(1 - d**2), d)  # 肘上解
        theta1 = np.arctan2(y, x) - np.arctan2(
            self.l2 * np.sin(theta2),
            self.l1 + self.l2 * np.cos(theta2)
        )
 
        return theta1, theta2
 
    def plan_trajectory(self, start, end, steps=50):
        """始点から終点への滑らかな軌道計画"""
        trajectory = []
        for t in np.linspace(0, 1, steps):
            # 3次補間で滑らかな動き
            s = 3*t**2 - 2*t**3  # smoothstep
            x = start[0] + s * (end[0] - start[0])
            y = start[1] + s * (end[1] - start[1])
            theta1, theta2 = self.inverse_kinematics(x, y)
            trajectory.append((theta1, theta2))
        return trajectory

import numpy as np
 
def dh_transform(theta, d, a, alpha):
    """
    Denavit-Hartenberg 変換行列
    theta: 関節角度 (回転関節)
    d: リンクオフセット
    a: リンク長
    alpha: リンクのねじり角
    """
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), np.sin(alpha)
 
    return np.array([
        [ct, -st*ca,  st*sa, a*ct],
        [st,  ct*ca, -ct*sa, a*st],
        [0,   sa,     ca,    d   ],
        [0,   0,      0,     1   ],
    ])
 
class Robot6DOF:
    """6自由度ロボットアーム（Puma 560 風 DH パラメータ）"""
 
    def __init__(self):
        # DH パラメータ [d, a, alpha] (theta は変数)
        self.dh_params = [
            [0.670, 0,     np.pi/2],   # Joint 1
            [0,     0.432, 0],          # Joint 2
            [0,     0.020, np.pi/2],    # Joint 3
            [0.432, 0,    -np.pi/2],    # Joint 4
            [0,     0,     np.pi/2],    # Joint 5
            [0.056, 0,     0],          # Joint 6
        ]
 
    def forward_kinematics(self, joint_angles):
        """順運動学: 6関節角度 → エンドエフェクタの4x4変換行列"""
        T = np.eye(4)
        for i, (d, a, alpha) in enumerate(self.dh_params):
            T = T @ dh_transform(joint_angles[i], d, a, alpha)
        return T
 
    def jacobian(self, joint_angles, delta=1e-6):
        """数値ヤコビアン: 関節速度 → エンドエフェクタ速度の変換"""
        J = np.zeros((6, 6))
        T0 = self.forward_kinematics(joint_angles)
        pos0 = T0[:3, 3]
 
        for i in range(6):
            angles_plus = joint_angles.copy()
            angles_plus[i] += delta
            T_plus = self.forward_kinematics(angles_plus)
 
            # 位置のヤコビアン
            J[:3, i] = (T_plus[:3, 3] - pos0) / delta
 
            # 姿勢のヤコビアン（簡略化）
            dR = T_plus[:3, :3] @ T0[:3, :3].T
            J[3:, i] = np.array([dR[2, 1], dR[0, 2], dR[1, 0]]) / delta
 
        return J
 
    def inverse_kinematics_numerical(self, target_pos, target_orient=None,
                                      max_iter=100, tol=1e-4):
        """数値的逆運動学（ヤコビアンベース）"""
        q = np.zeros(6)  # 初期姿勢
 
        for iteration in range(max_iter):
            T_current = self.forward_kinematics(q)
            pos_error = target_pos - T_current[:3, 3]
 
            if np.linalg.norm(pos_error) < tol:
                return q
 
            J = self.jacobian(q)
            J_pos = J[:3, :]  # 位置のヤコビアンのみ
 
            # ダンプ付き擬似逆行列（特異姿勢対策）
            lambda_sq = 0.01
            J_pinv = J_pos.T @ np.linalg.inv(
                J_pos @ J_pos.T + lambda_sq * np.eye(3)
            )
 
            dq = J_pinv @ pos_error
            q += dq * 0.5  # ステップサイズ
 
        return q

# 演習1: 基本実装のテンプレート
class Exercise1:
    """基本的な実装パターンの演習"""
 
    def __init__(self):
        self.data = []
 
    def validate_input(self, value):
        """入力値の検証"""
        if value is None:
            raise ValueError("入力値がNoneです")
        return True
 
    def process(self, value):
        """データ処理のメインロジック"""
        self.validate_input(value)
        self.data.append(value)
        return self.data
 
    def get_results(self):
        """処理結果の取得"""
        return {
            'count': len(self.data),
            'data': self.data
        }
 
# テスト
def test_exercise1():
    ex = Exercise1()
    assert ex.process(1) == [1]
    assert ex.process(2) == [1, 2]
    assert ex.get_results()['count'] == 2
 
    try:
        ex.process(None)
        assert False, "例外が発生するべき"
    except ValueError:
        pass
 
    print("全テスト合格!")
 
test_exercise1()

# 演習2: 応用パターン
from typing import List, Dict, Optional
from datetime import datetime
 
class AdvancedExercise:
    """応用パターンの演習"""
 
    def __init__(self, max_size: int = 100):
        self._items: List[Dict] = []
        self._max_size = max_size
        self._created_at = datetime.now()
 
    def add(self, key: str, value: any) -> bool:
        """アイテムの追加（サイズ制限付き）"""
        if len(self._items) >= self._max_size:
            return False
        self._items.append({
            'key': key,
            'value': value,
            'timestamp': datetime.now().isoformat()
        })
        return True
 
    def find(self, key: str) -> Optional[Dict]:
        """キーによる検索"""
        for item in reversed(self._items):
            if item['key'] == key:
                return item
        return None
 
    def remove(self, key: str) -> bool:
        """キーによる削除"""
        for i, item in enumerate(self._items):
            if item['key'] == key:
                self._items.pop(i)
                return True
        return False
 
    def stats(self) -> Dict:
        """統計情報"""
        return {
            'total_items': len(self._items),
            'max_size': self._max_size,
            'usage_percent': len(self._items) / self._max_size * 100,
            'uptime': str(datetime.now() - self._created_at)
        }
 
# テスト
def test_advanced():
    ex = AdvancedExercise(max_size=3)
    assert ex.add("a", 1) == True
    assert ex.add("b", 2) == True
    assert ex.add("c", 3) == True
    assert ex.add("d", 4) == False  # サイズ制限
    assert ex.find("b")['value'] == 2
    assert ex.remove("b") == True
    assert ex.find("b") is None
    stats = ex.stats()
    assert stats['total_items'] == 2
    print("応用テスト全合格!")
 
test_advanced()

# 演習3: パフォーマンス最適化
import time
from functools import lru_cache
 
# 最適化前（O(n^2)）
def slow_search(data: list, target: int) -> int:
    """非効率な検索"""
    for i in range(len(data)):
        for j in range(i + 1, len(data)):
            if data[i] + data[j] == target:
                return (i, j)
    return (-1, -1)
 
# 最適化後（O(n)）
def fast_search(data: list, target: int) -> tuple:
    """ハッシュマップを使った効率的な検索"""
    seen = {}
    for i, num in enumerate(data):
        complement = target - num
        if complement in seen:
            return (seen[complement], i)
        seen[num] = i
    return (-1, -1)
 
# ベンチマーク
def benchmark():
    import random
    data = list(range(5000))
    random.shuffle(data)
    target = data[100] + data[4000]
 
    start = time.time()
    result1 = slow_search(data, target)
    slow_time = time.time() - start
 
    start = time.time()
    result2 = fast_search(data, target)
    fast_time = time.time() - start
 
    print(f"非効率版: {slow_time:.4f}秒")
    print(f"効率版:   {fast_time:.6f}秒")
    print(f"高速化率: {slow_time/fast_time:.0f}倍")
 
benchmark()