File size: 4,028 Bytes
803f98a |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 |
# 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() |