로보틱스 & AI 완전 가이드: ROS2부터 휴머노이드 로봇까지

import numpy as np

def dh_transform(a, d, alpha, theta):
    """Denavit-Hartenberg 변환 행렬 계산"""
    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   ]
    ])

def forward_kinematics(joint_angles, dh_params):
    """
    순기구학: 관절 각도 -> 엔드이펙터 위치/자세
    dh_params: [(a, d, alpha, theta_offset), ...]
    """
    T = np.eye(4)
    for i, theta in enumerate(joint_angles):
        a, d, alpha, theta_off = dh_params[i]
        T = T @ dh_transform(a, d, alpha, theta + theta_off)
    return T

# 2-DOF 평면 로봇팔 예시 (링크 길이 각 1.0m)
dh_params_2dof = [
    (1.0, 0, 0, 0),
    (1.0, 0, 0, 0),
]
joint_angles = [np.pi/4, np.pi/4]
T_end = forward_kinematics(joint_angles, dh_params_2dof)
print("엔드이펙터 위치:", T_end[:3, 3])

import rclpy
from rclpy.node import Node
from std_msgs.msg import String

class TalkerNode(Node):
    def __init__(self):
        super().__init__('talker')
        self.publisher = self.create_publisher(String, 'chatter', 10)
        self.timer = self.create_timer(0.5, self.publish_msg)
        self.count = 0

    def publish_msg(self):
        msg = String()
        msg.data = f'Hello ROS2: {self.count}'
        self.publisher.publish(msg)
        self.get_logger().info(f'Published: {msg.data}')
        self.count += 1

class ListenerNode(Node):
    def __init__(self):
        super().__init__('listener')
        self.subscription = self.create_subscription(
            String, 'chatter', self.callback, 10
        )

    def callback(self, msg):
        self.get_logger().info(f'Received: {msg.data}')

def main():
    rclpy.init()
    talker = TalkerNode()
    rclpy.spin(talker)
    rclpy.shutdown()

# launch/my_robot.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='my_robot',
            executable='talker',
            name='talker_node',
            output='screen',
            parameters=[{'use_sim_time': False}],
        ),
        Node(
            package='my_robot',
            executable='listener',
            name='listener_node',
            output='screen',
        ),
    ])

import open3d as o3d
import numpy as np

def process_pointcloud(pcd_path):
    # 포인트 클라우드 로드
    pcd = o3d.io.read_point_cloud(pcd_path)

    # 통계적 이상치 제거
    pcd_clean, _ = pcd.remove_statistical_outlier(
        nb_neighbors=20, std_ratio=2.0
    )

    # Voxel downsampling (해상도 축소)
    pcd_down = pcd_clean.voxel_down_sample(voxel_size=0.05)

    # 법선 추정
    pcd_down.estimate_normals(
        search_param=o3d.geometry.KDTreeSearchParamHybrid(
            radius=0.1, max_nn=30
        )
    )

    # RANSAC 평면 분할 (지면 제거)
    plane_model, inliers = pcd_down.segment_plane(
        distance_threshold=0.01,
        ransac_n=3,
        num_iterations=1000
    )
    [a, b, c, d] = plane_model
    print(f"지면 평면 방정식: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")

    # 지면 제외 객체만 추출
    objects_pcd = pcd_down.select_by_index(inliers, invert=True)
    return objects_pcd

# Cartographer ROS2 설치 및 실행
sudo apt install ros-humble-cartographer-ros

# Cartographer 실행 (예: TurtleBot3)
ros2 launch turtlebot3_cartographer cartographer.launch.py

import numpy as np

def rrt_star(start, goal, obstacles, max_iter=1000, step=0.5, goal_radius=0.5):
    """
    간단한 2D RRT* 구현
    start, goal: np.array([x, y])
    obstacles: [(cx, cy, radius), ...]
    """
    nodes = [start]
    parents = [-1]
    costs = [0.0]

    def is_collision(p1, p2):
        """선분 p1-p2가 장애물과 충돌하는지 검사"""
        for (cx, cy, r) in obstacles:
            center = np.array([cx, cy])
            d = p2 - p1
            t = np.clip(np.dot(center - p1, d) / (np.dot(d, d) + 1e-9), 0, 1)
            closest = p1 + t * d
            if np.linalg.norm(closest - center) < r:
                return True
        return False

    for _ in range(max_iter):
        rand = goal if np.random.random() < 0.05 else np.random.uniform(-10, 10, 2)

        dists = [np.linalg.norm(np.array(n) - rand) for n in nodes]
        nearest_idx = int(np.argmin(dists))
        nearest = np.array(nodes[nearest_idx])

        direction = rand - nearest
        norm = np.linalg.norm(direction)
        if norm < 1e-9:
            continue
        new_node = nearest + step * direction / norm

        if is_collision(nearest, new_node):
            continue

        nodes.append(new_node)
        parents.append(nearest_idx)
        costs.append(costs[nearest_idx] + step)

        if np.linalg.norm(new_node - goal) < goal_radius:
            print(f"목표 도달! 총 노드 수: {len(nodes)}")
            return nodes, parents

    return nodes, parents

# 실행 예시
start = np.array([0.0, 0.0])
goal = np.array([9.0, 9.0])
obstacles = [(5.0, 5.0, 1.5), (3.0, 7.0, 1.0)]
nodes, parents = rrt_star(start, goal, obstacles)

# MoveIt2 Python API 예시
import rclpy
from moveit.planning import MoveItPy
from moveit.core.robot_state import RobotState

rclpy.init()
moveit = MoveItPy(node_name='moveit_py_planning')
panda_arm = moveit.get_planning_component('panda_arm')

# 목표 포즈 설정
from geometry_msgs.msg import PoseStamped
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'panda_link0'
goal_pose.pose.position.x = 0.5
goal_pose.pose.position.y = 0.0
goal_pose.pose.position.z = 0.5
goal_pose.pose.orientation.w = 1.0

panda_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='panda_hand')

# 경로 계획 및 실행
plan_result = panda_arm.plan()
if plan_result:
    moveit.execute(plan_result.trajectory)

from ultralytics import YOLO
import cv2

# YOLOv11 로드
model = YOLO('yolo11n.pt')

def detect_objects(frame):
    """카메라 프레임에서 물체 감지"""
    results = model(frame, conf=0.5)
    annotated = results[0].plot()

    for box in results[0].boxes:
        cls_id = int(box.cls[0])
        label = model.names[cls_id]
        conf = float(box.conf[0])
        x1, y1, x2, y2 = map(int, box.xyxy[0])
        print(f"{label}: {conf:.2f} at ({x1},{y1})-({x2},{y2})")

    return annotated

# 웹캠 실시간 감지
cap = cv2.VideoCapture(0)
while cap.isOpened():
    ret, frame = cap.read()
    if not ret:
        break
    annotated_frame = detect_objects(frame)
    cv2.imshow('Robot Vision', annotated_frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break
cap.release()

import cv2
import numpy as np

def estimate_pose(object_points, image_points, camera_matrix, dist_coeffs):
    """
    PnP 알고리즘으로 6D 포즈 추정
    object_points: 3D 점 (물체 좌표계)
    image_points: 2D 점 (이미지 좌표)
    """
    success, rvec, tvec = cv2.solvePnP(
        object_points, image_points,
        camera_matrix, dist_coeffs,
        flags=cv2.SOLVEPNP_ITERATIVE
    )
    if success:
        R, _ = cv2.Rodrigues(rvec)
        print(f"위치: {tvec.flatten()}")
        print(f"회전 행렬:\n{R}")
        return R, tvec
    return None, None

import gymnasium as gym
from stable_baselines3 import SAC
from stable_baselines3.common.callbacks import EvalCallback
import os

# MuJoCo Ant 환경에서 SAC 학습
env = gym.make('Ant-v4', render_mode=None)
eval_env = gym.make('Ant-v4', render_mode=None)

# 최적 모델 저장 콜백
eval_callback = EvalCallback(
    eval_env,
    best_model_save_path='./logs/best_model',
    log_path='./logs/',
    eval_freq=10000,
    deterministic=True,
    render=False
)

# SAC 모델 생성
model = SAC(
    'MlpPolicy',
    env,
    verbose=1,
    learning_rate=3e-4,
    buffer_size=1_000_000,
    batch_size=256,
    ent_coef='auto',
    gamma=0.99,
    tau=0.005,
    tensorboard_log='./tb_logs/'
)

# 학습
model.learn(total_timesteps=1_000_000, callback=eval_callback)
model.save('ant_sac_final')

# 추론 (배포)
obs, _ = env.reset()
total_reward = 0
for _ in range(1000):
    action, _ = model.predict(obs, deterministic=True)
    obs, reward, done, truncated, info = env.step(action)
    total_reward += reward
    if done or truncated:
        print(f"에피소드 종료, 총 보상: {total_reward:.2f}")
        obs, _ = env.reset()
        total_reward = 0

class AutonomousDrivingSystem:
    """자율주행 시스템의 기본 파이프라인"""

    def __init__(self, perception, prediction, planning, control):
        self.perception = perception    # 인식 모듈
        self.prediction = prediction    # 예측 모듈
        self.planning = planning        # 계획 모듈
        self.control = control          # 제어 모듈

    def run_step(self, sensor_data):
        # 1. 인식: 센서 데이터에서 객체 감지 및 추적
        detected_objects = self.perception.detect(sensor_data)
        ego_state = self.perception.localize(sensor_data)

        # 2. 예측: 주변 차량/보행자 미래 궤적 예측
        predicted_trajectories = self.prediction.predict(
            detected_objects, horizon=3.0  # 3초 예측
        )

        # 3. 계획: 목표까지 안전한 경로 계획
        trajectory = self.planning.plan(
            ego_state, predicted_trajectories, goal=self.goal
        )

        # 4. 제어: 계획된 경로 추종
        steering, throttle, brake = self.control.compute(
            ego_state, trajectory
        )

        return steering, throttle, brake

# OpenVLA 추론 예시 (개념 코드)
from transformers import AutoModelForVision2Seq, AutoProcessor
import torch

model_name = "openvla/openvla-7b"
processor = AutoProcessor.from_pretrained(model_name, trust_remote_code=True)
model = AutoModelForVision2Seq.from_pretrained(
    model_name,
    torch_dtype=torch.bfloat16,
    device_map='auto',
    trust_remote_code=True
)

# 비전-언어 명령으로 행동 생성
image = get_camera_image()  # 로봇 카메라 이미지
instruction = "pick up the red block and place it in the box"

inputs = processor(instruction, image).to('cuda:0', dtype=torch.bfloat16)
action = model.predict_action(**inputs, unnorm_key='bridge_orig', do_sample=False)
# action: [dx, dy, dz, droll, dpitch, dyaw, gripper]
robot.execute_action(action)