Robotics & AI Complete Guide: From ROS2 to Humanoid Robots

import numpy as np

def dh_transform(a, d, alpha, theta):
    """Compute a Denavit-Hartenberg transformation matrix."""
    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):
    """
    FK: joint angles -> end-effector pose
    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 planar arm example (link lengths 1.0m each)
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("End-effector position:", 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()
    node = TalkerNode()
    rclpy.spin(node)
    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):
    # Load point cloud
    pcd = o3d.io.read_point_cloud(pcd_path)

    # Statistical outlier removal
    pcd_clean, _ = pcd.remove_statistical_outlier(
        nb_neighbors=20, std_ratio=2.0
    )

    # Voxel downsampling (reduce resolution)
    pcd_down = pcd_clean.voxel_down_sample(voxel_size=0.05)

    # Normal estimation
    pcd_down.estimate_normals(
        search_param=o3d.geometry.KDTreeSearchParamHybrid(
            radius=0.1, max_nn=30
        )
    )

    # RANSAC plane segmentation (ground removal)
    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"Ground plane: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")

    # Extract non-ground objects
    objects_pcd = pcd_down.select_by_index(inliers, invert=True)
    return objects_pcd

# Install Cartographer for ROS2 Humble
sudo apt install ros-humble-cartographer-ros

# Run Cartographer with 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):
    """
    Simple 2D RRT* implementation.
    start, goal: np.array([x, y])
    obstacles: [(cx, cy, radius), ...]
    """
    nodes = [start]
    parents = [-1]
    costs = [0.0]

    def is_collision(p1, p2):
        """Check if segment p1-p2 intersects any obstacle."""
        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"Goal reached! Total nodes: {len(nodes)}")
            return nodes, parents

    return nodes, parents

# Example usage
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 example
import rclpy
from moveit.planning import MoveItPy

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

# Set goal pose
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 and execute
plan_result = panda_arm.plan()
if plan_result:
    moveit.execute(plan_result.trajectory)

from ultralytics import YOLO
import cv2

# Load YOLOv11 nano
model = YOLO('yolo11n.pt')

def detect_objects(frame):
    """Detect objects in a camera 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

# Live webcam detection
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):
    """
    6D pose estimation using PnP.
    object_points: 3D points in object coordinate frame
    image_points: Corresponding 2D image coordinates
    """
    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"Position: {tvec.flatten()}")
        print(f"Rotation matrix:\n{R}")
        return R, tvec
    return None, None

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

# Train SAC on MuJoCo Ant
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
)

model = SAC(
    'MlpPolicy',
    env,
    verbose=1,
    learning_rate=3e-4,
    buffer_size=1_000_000,
    batch_size=256,
    ent_coef='auto',  # auto-tune entropy coefficient
    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')

# Inference / deployment
obs, _ = env.reset()
total_reward = 0.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"Episode ended. Total reward: {total_reward:.2f}")
        obs, _ = env.reset()
        total_reward = 0.0

class AutonomousDrivingSystem:
    """Standard autonomous driving pipeline."""

    def __init__(self, perception, prediction, planning, control):
        self.perception = perception
        self.prediction = prediction
        self.planning = planning
        self.control = control
        self.goal = None

    def run_step(self, sensor_data):
        # 1. Perception: detect and track objects, localize ego vehicle
        detected_objects = self.perception.detect(sensor_data)
        ego_state = self.perception.localize(sensor_data)

        # 2. Prediction: forecast future trajectories of other agents
        predicted_trajectories = self.prediction.predict(
            detected_objects, horizon=3.0  # 3-second prediction horizon
        )

        # 3. Planning: generate a safe, comfortable trajectory to goal
        trajectory = self.planning.plan(
            ego_state, predicted_trajectories, goal=self.goal
        )

        # 4. Control: track the planned trajectory
        steering, throttle, brake = self.control.compute(
            ego_state, trajectory
        )

        return steering, throttle, brake

# OpenVLA inference example (conceptual)
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
)

# Generate robot action from vision + language
image = get_camera_image()   # robot wrist 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)