app.py

# Install required libraries

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import gradio as gr
from scipy.optimize import minimize

# Define forward kinematics (simplified example)
def forward_kinematics(theta):
    # Replace with actual FK for your robot
    # Example: Simple 3D position calculation
    x = np.cos(theta[0]) + np.cos(theta[1]) + np.cos(theta[2])
    y = np.sin(theta[0]) + np.sin(theta[1]) + np.sin(theta[2])
    z = theta[3] + theta[4] + theta[5]
    return np.array([x, y, z])

# Define obstacle positions (example)
obstacles = [
    {"position": np.array([0.5, 0.5, 0.5]), "radius": 0.2},
    {"position": np.array([1.0, 1.0, 1.0]), "radius": 0.3}
]

# Check for collisions with obstacles
def is_collision(position):
    for obstacle in obstacles:
        obstacle_position = obstacle["position"]
        obstacle_radius = obstacle["radius"]
        distance = np.linalg.norm(position - obstacle_position)
        if distance < obstacle_radius:
            return True  # Collision detected
    return False  # No collision

# Define inverse kinematics with obstacle avoidance
def inverse_kinematics(target_position, initial_angles):
    def error_function(theta):
        current_position = forward_kinematics(theta)
        # Attractive force to target
        error = np.linalg.norm(target_position - current_position)
        # Add penalty for collisions
        if is_collision(current_position):
            error += 1000  # Large penalty for collisions
        return error
    
    # Use scipy.optimize to minimize the error
    result = minimize(error_function, initial_angles, method='BFGS')
    return result.x

# Generate trajectory (from initial to target position)
def generate_trajectory(target_position):
    initial_angles = [0, 0, 0, 0, 0, 0]  # Initial joint angles
    target_angles = inverse_kinematics(target_position, initial_angles)
    # Interpolate between initial and target angles (fewer points for faster processing)
    trajectory = np.linspace(initial_angles, target_angles, 50)  # Reduced to 50 points
    return trajectory

# Plot the trajectory in 3D with obstacles
def plot_trajectory(trajectory):
    x, y, z = [], [], []
    for theta in trajectory:
        position = forward_kinematics(theta)
        x.append(position[0])
        y.append(position[1])
        z.append(position[2])
    
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    ax.plot(x, y, z, label="Robot Trajectory")
    ax.scatter(x[0], y[0], z[0], color='green', label="Start")
    ax.scatter(x[-1], y[-1], z[-1], color='red', label="Target")
    
    # Plot obstacles
    for obstacle in obstacles:
        obstacle_position = obstacle["position"]
        obstacle_radius = obstacle["radius"]
        u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
        x_obs = obstacle_position[0] + obstacle_radius * np.cos(u) * np.sin(v)
        y_obs = obstacle_position[1] + obstacle_radius * np.sin(u) * np.sin(v)
        z_obs = obstacle_position[2] + obstacle_radius * np.cos(v)
        ax.plot_wireframe(x_obs, y_obs, z_obs, color='orange', alpha=0.5, label="Obstacle" if obstacle == obstacles[0] else "")
    
    ax.set_xlabel("X")
    ax.set_ylabel("Y")
    ax.set_zlabel("Z")
    ax.legend()
    return fig

# Gradio Interface
def gradio_interface(x, y, z):
    target_position = np.array([x, y, z])
    
    # Generate trajectory
    trajectory = generate_trajectory(target_position)
    
    # Plot trajectory
    fig = plot_trajectory(trajectory)
    return fig

# Launch Gradio App
iface = gr.Interface(
    fn=gradio_interface,
    inputs=[
        gr.Number(label="Target X"),
        gr.Number(label="Target Y"),
        gr.Number(label="Target Z")
    ],
    outputs="plot",
    live=False,  # Disable live updates to show Submit button
    title="6-DOF Robot Path Planning with Obstacle Avoidance",
    description="Enter the target (x, y, z) position and click Submit to generate the optimized trajectory."
)

iface.launch()