AI core code examples

import pybullet as p
import numpy as np

# Initialize physics simulation
physics_client = p.connect(p.GUI)  # Enable visualization
p.setGravity(0, 0, -9.8)
plane_id = p.loadURDF("plane.urdf")
cube_id = p.loadURDF("cube.urdf", [0, 0, 2])

# PID controller for position tracking
def pid_control(target_pos, current_pos, kp=5.0, ki=0.01, kd=0.1):
    error = np.array(target_pos) - np.array(current_pos)
    integral = np.zeros(3)
    derivative = np.zeros(3)
    # PID calculation
    force = kp * error + ki * integral + kd * derivative
    return force

# Main simulation loop
for _ in range(1000):
    cube_pos, _ = p.getBasePositionAndOrientation(cube_id)
    force = pid_control([1, 0, 1], cube_pos)
    p.applyExternalForce(cube_id, -1, force, cube_pos, p.WORLD_FRAME)
    p.stepSimulation()

from sklearn.neighbors import KDTree
import networkx as nx

class RobotNavigator:
    def __init__(self, obstacle_map):
        self.graph = nx.Graph()
        self.kdtree = KDTree(obstacle_map)  # Obstacle coordinates
        
    def generate_path(self, start, goal, clearance=0.5):
        # Add nodes with obstacle avoidance
        if self.kdtree.query_radius([start], r=clearance)[0].size == 0:
            self.graph.add_node(start)
        # Simplified A* implementation
        path = nx.astar_path(self.graph, start, goal, heuristic=self.euclidean_dist)
        return path

    @staticmethod
    def euclidean_dist(a, b):
        return np.linalg.norm(np.array(a) - np.array(b))

# Usage:
obstacles = np.array([[1,2], [3,4], [5,6]])  # From environment scan
nav = RobotNavigator(obstacles)
path = nav.generate_path(start=(0,0), goal=(10,10))

import tensorflow as tf
from tensorflow.keras.layers import Dense

class DQNAgent:
    def __init__(self, state_dim, action_dim):
        self.model = tf.keras.Sequential([
            Dense(128, activation='relu', input_shape=(state_dim,)),
            Dense(64, activation='relu'),
            Dense(action_dim, activation='linear')
        ])
        self.model.compile(optimizer=tf.keras.optimizers.Adam(0.001), loss='mse')

    def predict_action(self, state, epsilon=0.1):
        if np.random.rand() < epsilon:
            return np.random.randint(self.action_dim)  # Exploration
        q_values = self.model.predict(state[np.newaxis])
        return np.argmax(q_values[0])

# Training scenario: Avoid collisions
def train_agent(env, episodes=1000):
    agent = DQNAgent(state_dim=env.obs_size, action_dim=4)  # Actions: [forward, left, right, stop]
    for episode in range(episodes):
        state = env.reset()
        while not env.done:
            action = agent.predict_action(state)
            next_state, reward, done = env.step(action)
            # Update model with experience replay
            agent.model.fit(state, reward, epochs=1, verbose=0)
            state = next_state

class VirtualHardware:
    def __init__(self, device_type):
        self.device = self._load_driver(device_type)
        
    def _load_driver(self, type):
        # ROS 2 integration for hardware control
        if type == "Arduino":
            from ros_arduino_interfacer import ArduinoDriver
            return ArduinoDriver()
        elif type == "RaspberryPi":
            from ros_pi_interfacer import PiGPIO
            return PiGPIO()
    
    def execute(self, command, params):
        return self.device.send_command(command, **params)

# Usage with robotics API
motor = VirtualHardware("Arduino")
motor.execute("SET_SPEED", {"motor_id": 2, "rpm": 150})