Create app.py

app.py

@@ -0,0 +1,114 @@
+# Install required libraries
+!pip install numpy matplotlib gradio scipy
+
+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()