Spaces:
Sleeping
Sleeping
Create app.py
Browse files
app.py
ADDED
@@ -0,0 +1,1414 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
1 |
+
import numpy as np
|
2 |
+
import matplotlib.pyplot as plt
|
3 |
+
from mpl_toolkits.mplot3d import Axes3D
|
4 |
+
import gradio as gr
|
5 |
+
from scipy.optimize import minimize
|
6 |
+
|
7 |
+
# Define forward kinematics (simplified example)
|
8 |
+
def forward_kinematics(theta):
|
9 |
+
x = np.cos(theta[0]) + np.cos(theta[1]) + np.cos(theta[2])
|
10 |
+
y = np.sin(theta[0]) + np.sin(theta[1]) + np.sin(theta[2])
|
11 |
+
z = theta[3] + theta[4] + theta[5]
|
12 |
+
return np.array([x, y, z])
|
13 |
+
|
14 |
+
# Define obstacle positions (example)
|
15 |
+
obstacles = [
|
16 |
+
{"position": np.array([0.5, 0.5, 0.5]), "radius": 0.2},
|
17 |
+
{"position": np.array([1.0, 1.0, 1.0]), "radius": 0.3}
|
18 |
+
]
|
19 |
+
|
20 |
+
# Check for collisions with obstacles
|
21 |
+
def is_collision(position):
|
22 |
+
for obstacle in obstacles:
|
23 |
+
obstacle_position = obstacle["position"]
|
24 |
+
obstacle_radius = obstacle["radius"]
|
25 |
+
distance = np.linalg.norm(position - obstacle_position)
|
26 |
+
if distance < obstacle_radius:
|
27 |
+
return True # Collision detected
|
28 |
+
return False # No collision
|
29 |
+
|
30 |
+
# Define inverse kinematics with obstacle avoidance
|
31 |
+
def inverse_kinematics(target_position, initial_angles):
|
32 |
+
def error_function(theta):
|
33 |
+
current_position = forward_kinematics(theta)
|
34 |
+
error = np.linalg.norm(target_position - current_position)
|
35 |
+
if is_collision(current_position):
|
36 |
+
error += 1000 # Large penalty for collisions
|
37 |
+
return error
|
38 |
+
|
39 |
+
result = minimize(error_function, initial_angles, method='BFGS')
|
40 |
+
return result.x
|
41 |
+
|
42 |
+
# Generate trajectory (from initial to target position)
|
43 |
+
def generate_trajectory(target_position):
|
44 |
+
initial_angles = [0, 0, 0, 0, 0, 0] # Initial joint angles
|
45 |
+
target_angles = inverse_kinematics(target_position, initial_angles)
|
46 |
+
trajectory = np.linspace(initial_angles, target_angles, 50) # Reduced to 50 points
|
47 |
+
return trajectory
|
48 |
+
|
49 |
+
# Plot the trajectory in 3D with obstacles
|
50 |
+
def plot_trajectory(trajectory):
|
51 |
+
x, y, z = [], [], []
|
52 |
+
for theta in trajectory:
|
53 |
+
position = forward_kinematics(theta)
|
54 |
+
x.append(position[0])
|
55 |
+
y.append(position[1])
|
56 |
+
z.append(position[2])
|
57 |
+
|
58 |
+
fig = plt.figure()
|
59 |
+
ax = fig.add_subplot(111, projection='3d')
|
60 |
+
ax.plot(x, y, z, label="Robot Trajectory")
|
61 |
+
ax.scatter(x[0], y[0], z[0], color='green', label="Start")
|
62 |
+
ax.scatter(x[-1], y[-1], z[-1], color='red', label="Target")
|
63 |
+
|
64 |
+
# Plot obstacles
|
65 |
+
for obstacle in obstacles:
|
66 |
+
obstacle_position = obstacle["position"]
|
67 |
+
obstacle_radius = obstacle["radius"]
|
68 |
+
u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
|
69 |
+
x_obs = obstacle_position[0] + obstacle_radius * np.cos(u) * np.sin(v)
|
70 |
+
y_obs = obstacle_position[1] + obstacle_radius * np.sin(u) * np.sin(v)
|
71 |
+
z_obs = obstacle_position[2] + obstacle_radius * np.cos(v)
|
72 |
+
ax.plot_wireframe(x_obs, y_obs, z_obs, color='orange', alpha=0.5, label="Obstacle" if obstacle == obstacles[0] else "")
|
73 |
+
|
74 |
+
ax.set_xlabel("X")
|
75 |
+
ax.set_ylabel("Y")
|
76 |
+
ax.set_zlabel("Z")
|
77 |
+
ax.legend()
|
78 |
+
return fig
|
79 |
+
|
80 |
+
# Gradio Interface
|
81 |
+
def gradio_interface(x, y, z):
|
82 |
+
target_position = np.array([x, y, z])
|
83 |
+
trajectory = generate_trajectory(target_position)
|
84 |
+
fig = plot_trajectory(trajectory)
|
85 |
+
return fig
|
86 |
+
|
87 |
+
# Launch Gradio App
|
88 |
+
iface = gr.Interface(
|
89 |
+
fn=gradio_interface,
|
90 |
+
inputs=[
|
91 |
+
gr.Number(label="Target X"),
|
92 |
+
gr.Number(label="Target Y"),
|
93 |
+
gr.Number(label="Target Z")
|
94 |
+
],
|
95 |
+
outputs="plot",
|
96 |
+
live=False,
|
97 |
+
title="6-DOF Robot Path Planning with Obstacle Avoidance",
|
98 |
+
description="Enter the target (x, y, z) position and click Submit to generate the optimized trajectory."
|
99 |
+
)
|
100 |
+
|
101 |
+
iface.launch()# app.py
|
102 |
+
import numpy as np
|
103 |
+
import matplotlib.pyplot as plt
|
104 |
+
from mpl_toolkits.mplot3d import Axes3D
|
105 |
+
import gradio as gr
|
106 |
+
from scipy.optimize import minimize
|
107 |
+
|
108 |
+
# Define forward kinematics (simplified example)
|
109 |
+
def forward_kinematics(theta):
|
110 |
+
x = np.cos(theta[0]) + np.cos(theta[1]) + np.cos(theta[2])
|
111 |
+
y = np.sin(theta[0]) + np.sin(theta[1]) + np.sin(theta[2])
|
112 |
+
z = theta[3] + theta[4] + theta[5]
|
113 |
+
return np.array([x, y, z])
|
114 |
+
|
115 |
+
# Define obstacle positions (example)
|
116 |
+
obstacles = [
|
117 |
+
{"position": np.array([0.5, 0.5, 0.5]), "radius": 0.2},
|
118 |
+
{"position": np.array([1.0, 1.0, 1.0]), "radius": 0.3}
|
119 |
+
]
|
120 |
+
|
121 |
+
# Check for collisions with obstacles
|
122 |
+
def is_collision(position):
|
123 |
+
for obstacle in obstacles:
|
124 |
+
obstacle_position = obstacle["position"]
|
125 |
+
obstacle_radius = obstacle["radius"]
|
126 |
+
distance = np.linalg.norm(position - obstacle_position)
|
127 |
+
if distance < obstacle_radius:
|
128 |
+
return True # Collision detected
|
129 |
+
return False # No collision
|
130 |
+
|
131 |
+
# Define inverse kinematics with obstacle avoidance
|
132 |
+
def inverse_kinematics(target_position, initial_angles):
|
133 |
+
def error_function(theta):
|
134 |
+
current_position = forward_kinematics(theta)
|
135 |
+
error = np.linalg.norm(target_position - current_position)
|
136 |
+
if is_collision(current_position):
|
137 |
+
error += 1000 # Large penalty for collisions
|
138 |
+
return error
|
139 |
+
|
140 |
+
result = minimize(error_function, initial_angles, method='BFGS')
|
141 |
+
return result.x
|
142 |
+
|
143 |
+
# Generate trajectory (from initial to target position)
|
144 |
+
def generate_trajectory(target_position):
|
145 |
+
initial_angles = [0, 0, 0, 0, 0, 0] # Initial joint angles
|
146 |
+
target_angles = inverse_kinematics(target_position, initial_angles)
|
147 |
+
trajectory = np.linspace(initial_angles, target_angles, 50) # Reduced to 50 points
|
148 |
+
return trajectory
|
149 |
+
|
150 |
+
# Plot the trajectory in 3D with obstacles
|
151 |
+
def plot_trajectory(trajectory):
|
152 |
+
x, y, z = [], [], []
|
153 |
+
for theta in trajectory:
|
154 |
+
position = forward_kinematics(theta)
|
155 |
+
x.append(position[0])
|
156 |
+
y.append(position[1])
|
157 |
+
z.append(position[2])
|
158 |
+
|
159 |
+
fig = plt.figure()
|
160 |
+
ax = fig.add_subplot(111, projection='3d')
|
161 |
+
ax.plot(x, y, z, label="Robot Trajectory")
|
162 |
+
ax.scatter(x[0], y[0], z[0], color='green', label="Start")
|
163 |
+
ax.scatter(x[-1], y[-1], z[-1], color='red', label="Target")
|
164 |
+
|
165 |
+
# Plot obstacles
|
166 |
+
for obstacle in obstacles:
|
167 |
+
obstacle_position = obstacle["position"]
|
168 |
+
obstacle_radius = obstacle["radius"]
|
169 |
+
u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
|
170 |
+
x_obs = obstacle_position[0] + obstacle_radius * np.cos(u) * np.sin(v)
|
171 |
+
y_obs = obstacle_position[1] + obstacle_radius * np.sin(u) * np.sin(v)
|
172 |
+
z_obs = obstacle_position[2] + obstacle_radius * np.cos(v)
|
173 |
+
ax.plot_wireframe(x_obs, y_obs, z_obs, color='orange', alpha=0.5, label="Obstacle" if obstacle == obstacles[0] else "")
|
174 |
+
|
175 |
+
ax.set_xlabel("X")
|
176 |
+
ax.set_ylabel("Y")
|
177 |
+
ax.set_zlabel("Z")
|
178 |
+
ax.legend()
|
179 |
+
return fig
|
180 |
+
|
181 |
+
# Gradio Interface
|
182 |
+
def gradio_interface(x, y, z):
|
183 |
+
target_position = np.array([x, y, z])
|
184 |
+
trajectory = generate_trajectory(target_position)
|
185 |
+
fig = plot_trajectory(trajectory)
|
186 |
+
return fig
|
187 |
+
|
188 |
+
# Launch Gradio App
|
189 |
+
iface = gr.Interface(
|
190 |
+
fn=gradio_interface,
|
191 |
+
inputs=[
|
192 |
+
gr.Number(label="Target X"),
|
193 |
+
gr.Number(label="Target Y"),
|
194 |
+
gr.Number(label="Target Z")
|
195 |
+
],
|
196 |
+
outputs="plot",
|
197 |
+
live=False,
|
198 |
+
title="6-DOF Robot Path Planning with Obstacle Avoidance",
|
199 |
+
description="Enter the target (x, y, z) position and click Submit to generate the optimized trajectory."
|
200 |
+
)
|
201 |
+
|
202 |
+
iface.launch()# app.py
|
203 |
+
import numpy as np
|
204 |
+
import matplotlib.pyplot as plt
|
205 |
+
from mpl_toolkits.mplot3d import Axes3D
|
206 |
+
import gradio as gr
|
207 |
+
from scipy.optimize import minimize
|
208 |
+
|
209 |
+
# Define forward kinematics (simplified example)
|
210 |
+
def forward_kinematics(theta):
|
211 |
+
x = np.cos(theta[0]) + np.cos(theta[1]) + np.cos(theta[2])
|
212 |
+
y = np.sin(theta[0]) + np.sin(theta[1]) + np.sin(theta[2])
|
213 |
+
z = theta[3] + theta[4] + theta[5]
|
214 |
+
return np.array([x, y, z])
|
215 |
+
|
216 |
+
# Define obstacle positions (example)
|
217 |
+
obstacles = [
|
218 |
+
{"position": np.array([0.5, 0.5, 0.5]), "radius": 0.2},
|
219 |
+
{"position": np.array([1.0, 1.0, 1.0]), "radius": 0.3}
|
220 |
+
]
|
221 |
+
|
222 |
+
# Check for collisions with obstacles
|
223 |
+
def is_collision(position):
|
224 |
+
for obstacle in obstacles:
|
225 |
+
obstacle_position = obstacle["position"]
|
226 |
+
obstacle_radius = obstacle["radius"]
|
227 |
+
distance = np.linalg.norm(position - obstacle_position)
|
228 |
+
if distance < obstacle_radius:
|
229 |
+
return True # Collision detected
|
230 |
+
return False # No collision
|
231 |
+
|
232 |
+
# Define inverse kinematics with obstacle avoidance
|
233 |
+
def inverse_kinematics(target_position, initial_angles):
|
234 |
+
def error_function(theta):
|
235 |
+
current_position = forward_kinematics(theta)
|
236 |
+
error = np.linalg.norm(target_position - current_position)
|
237 |
+
if is_collision(current_position):
|
238 |
+
error += 1000 # Large penalty for collisions
|
239 |
+
return error
|
240 |
+
|
241 |
+
result = minimize(error_function, initial_angles, method='BFGS')
|
242 |
+
return result.x
|
243 |
+
|
244 |
+
# Generate trajectory (from initial to target position)
|
245 |
+
def generate_trajectory(target_position):
|
246 |
+
initial_angles = [0, 0, 0, 0, 0, 0] # Initial joint angles
|
247 |
+
target_angles = inverse_kinematics(target_position, initial_angles)
|
248 |
+
trajectory = np.linspace(initial_angles, target_angles, 50) # Reduced to 50 points
|
249 |
+
return trajectory
|
250 |
+
|
251 |
+
# Plot the trajectory in 3D with obstacles
|
252 |
+
def plot_trajectory(trajectory):
|
253 |
+
x, y, z = [], [], []
|
254 |
+
for theta in trajectory:
|
255 |
+
position = forward_kinematics(theta)
|
256 |
+
x.append(position[0])
|
257 |
+
y.append(position[1])
|
258 |
+
z.append(position[2])
|
259 |
+
|
260 |
+
fig = plt.figure()
|
261 |
+
ax = fig.add_subplot(111, projection='3d')
|
262 |
+
ax.plot(x, y, z, label="Robot Trajectory")
|
263 |
+
ax.scatter(x[0], y[0], z[0], color='green', label="Start")
|
264 |
+
ax.scatter(x[-1], y[-1], z[-1], color='red', label="Target")
|
265 |
+
|
266 |
+
# Plot obstacles
|
267 |
+
for obstacle in obstacles:
|
268 |
+
obstacle_position = obstacle["position"]
|
269 |
+
obstacle_radius = obstacle["radius"]
|
270 |
+
u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
|
271 |
+
x_obs = obstacle_position[0] + obstacle_radius * np.cos(u) * np.sin(v)
|
272 |
+
y_obs = obstacle_position[1] + obstacle_radius * np.sin(u) * np.sin(v)
|
273 |
+
z_obs = obstacle_position[2] + obstacle_radius * np.cos(v)
|
274 |
+
ax.plot_wireframe(x_obs, y_obs, z_obs, color='orange', alpha=0.5, label="Obstacle" if obstacle == obstacles[0] else "")
|
275 |
+
|
276 |
+
ax.set_xlabel("X")
|
277 |
+
ax.set_ylabel("Y")
|
278 |
+
ax.set_zlabel("Z")
|
279 |
+
ax.legend()
|
280 |
+
return fig
|
281 |
+
|
282 |
+
# Gradio Interface
|
283 |
+
def gradio_interface(x, y, z):
|
284 |
+
target_position = np.array([x, y, z])
|
285 |
+
trajectory = generate_trajectory(target_position)
|
286 |
+
fig = plot_trajectory(trajectory)
|
287 |
+
return fig
|
288 |
+
|
289 |
+
# Launch Gradio App
|
290 |
+
iface = gr.Interface(
|
291 |
+
fn=gradio_interface,
|
292 |
+
inputs=[
|
293 |
+
gr.Number(label="Target X"),
|
294 |
+
gr.Number(label="Target Y"),
|
295 |
+
gr.Number(label="Target Z")
|
296 |
+
],
|
297 |
+
outputs="plot",
|
298 |
+
live=False,
|
299 |
+
title="6-DOF Robot Path Planning with Obstacle Avoidance",
|
300 |
+
description="Enter the target (x, y, z) position and click Submit to generate the optimized trajectory."
|
301 |
+
)
|
302 |
+
|
303 |
+
iface.launch()# app.py
|
304 |
+
import numpy as np
|
305 |
+
import matplotlib.pyplot as plt
|
306 |
+
from mpl_toolkits.mplot3d import Axes3D
|
307 |
+
import gradio as gr
|
308 |
+
from scipy.optimize import minimize
|
309 |
+
|
310 |
+
# Define forward kinematics (simplified example)
|
311 |
+
def forward_kinematics(theta):
|
312 |
+
x = np.cos(theta[0]) + np.cos(theta[1]) + np.cos(theta[2])
|
313 |
+
y = np.sin(theta[0]) + np.sin(theta[1]) + np.sin(theta[2])
|
314 |
+
z = theta[3] + theta[4] + theta[5]
|
315 |
+
return np.array([x, y, z])
|
316 |
+
|
317 |
+
# Define obstacle positions (example)
|
318 |
+
obstacles = [
|
319 |
+
{"position": np.array([0.5, 0.5, 0.5]), "radius": 0.2},
|
320 |
+
{"position": np.array([1.0, 1.0, 1.0]), "radius": 0.3}
|
321 |
+
]
|
322 |
+
|
323 |
+
# Check for collisions with obstacles
|
324 |
+
def is_collision(position):
|
325 |
+
for obstacle in obstacles:
|
326 |
+
obstacle_position = obstacle["position"]
|
327 |
+
obstacle_radius = obstacle["radius"]
|
328 |
+
distance = np.linalg.norm(position - obstacle_position)
|
329 |
+
if distance < obstacle_radius:
|
330 |
+
return True # Collision detected
|
331 |
+
return False # No collision
|
332 |
+
|
333 |
+
# Define inverse kinematics with obstacle avoidance
|
334 |
+
def inverse_kinematics(target_position, initial_angles):
|
335 |
+
def error_function(theta):
|
336 |
+
current_position = forward_kinematics(theta)
|
337 |
+
error = np.linalg.norm(target_position - current_position)
|
338 |
+
if is_collision(current_position):
|
339 |
+
error += 1000 # Large penalty for collisions
|
340 |
+
return error
|
341 |
+
|
342 |
+
result = minimize(error_function, initial_angles, method='BFGS')
|
343 |
+
return result.x
|
344 |
+
|
345 |
+
# Generate trajectory (from initial to target position)
|
346 |
+
def generate_trajectory(target_position):
|
347 |
+
initial_angles = [0, 0, 0, 0, 0, 0] # Initial joint angles
|
348 |
+
target_angles = inverse_kinematics(target_position, initial_angles)
|
349 |
+
trajectory = np.linspace(initial_angles, target_angles, 50) # Reduced to 50 points
|
350 |
+
return trajectory
|
351 |
+
|
352 |
+
# Plot the trajectory in 3D with obstacles
|
353 |
+
def plot_trajectory(trajectory):
|
354 |
+
x, y, z = [], [], []
|
355 |
+
for theta in trajectory:
|
356 |
+
position = forward_kinematics(theta)
|
357 |
+
x.append(position[0])
|
358 |
+
y.append(position[1])
|
359 |
+
z.append(position[2])
|
360 |
+
|
361 |
+
fig = plt.figure()
|
362 |
+
ax = fig.add_subplot(111, projection='3d')
|
363 |
+
ax.plot(x, y, z, label="Robot Trajectory")
|
364 |
+
ax.scatter(x[0], y[0], z[0], color='green', label="Start")
|
365 |
+
ax.scatter(x[-1], y[-1], z[-1], color='red', label="Target")
|
366 |
+
|
367 |
+
# Plot obstacles
|
368 |
+
for obstacle in obstacles:
|
369 |
+
obstacle_position = obstacle["position"]
|
370 |
+
obstacle_radius = obstacle["radius"]
|
371 |
+
u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
|
372 |
+
x_obs = obstacle_position[0] + obstacle_radius * np.cos(u) * np.sin(v)
|
373 |
+
y_obs = obstacle_position[1] + obstacle_radius * np.sin(u) * np.sin(v)
|
374 |
+
z_obs = obstacle_position[2] + obstacle_radius * np.cos(v)
|
375 |
+
ax.plot_wireframe(x_obs, y_obs, z_obs, color='orange', alpha=0.5, label="Obstacle" if obstacle == obstacles[0] else "")
|
376 |
+
|
377 |
+
ax.set_xlabel("X")
|
378 |
+
ax.set_ylabel("Y")
|
379 |
+
ax.set_zlabel("Z")
|
380 |
+
ax.legend()
|
381 |
+
return fig
|
382 |
+
|
383 |
+
# Gradio Interface
|
384 |
+
def gradio_interface(x, y, z):
|
385 |
+
target_position = np.array([x, y, z])
|
386 |
+
trajectory = generate_trajectory(target_position)
|
387 |
+
fig = plot_trajectory(trajectory)
|
388 |
+
return fig
|
389 |
+
|
390 |
+
# Launch Gradio App
|
391 |
+
iface = gr.Interface(
|
392 |
+
fn=gradio_interface,
|
393 |
+
inputs=[
|
394 |
+
gr.Number(label="Target X"),
|
395 |
+
gr.Number(label="Target Y"),
|
396 |
+
gr.Number(label="Target Z")
|
397 |
+
],
|
398 |
+
outputs="plot",
|
399 |
+
live=False,
|
400 |
+
title="6-DOF Robot Path Planning with Obstacle Avoidance",
|
401 |
+
description="Enter the target (x, y, z) position and click Submit to generate the optimized trajectory."
|
402 |
+
)
|
403 |
+
|
404 |
+
iface.launch()# app.py
|
405 |
+
import numpy as np
|
406 |
+
import matplotlib.pyplot as plt
|
407 |
+
from mpl_toolkits.mplot3d import Axes3D
|
408 |
+
import gradio as gr
|
409 |
+
from scipy.optimize import minimize
|
410 |
+
|
411 |
+
# Define forward kinematics (simplified example)
|
412 |
+
def forward_kinematics(theta):
|
413 |
+
x = np.cos(theta[0]) + np.cos(theta[1]) + np.cos(theta[2])
|
414 |
+
y = np.sin(theta[0]) + np.sin(theta[1]) + np.sin(theta[2])
|
415 |
+
z = theta[3] + theta[4] + theta[5]
|
416 |
+
return np.array([x, y, z])
|
417 |
+
|
418 |
+
# Define obstacle positions (example)
|
419 |
+
obstacles = [
|
420 |
+
{"position": np.array([0.5, 0.5, 0.5]), "radius": 0.2},
|
421 |
+
{"position": np.array([1.0, 1.0, 1.0]), "radius": 0.3}
|
422 |
+
]
|
423 |
+
|
424 |
+
# Check for collisions with obstacles
|
425 |
+
def is_collision(position):
|
426 |
+
for obstacle in obstacles:
|
427 |
+
obstacle_position = obstacle["position"]
|
428 |
+
obstacle_radius = obstacle["radius"]
|
429 |
+
distance = np.linalg.norm(position - obstacle_position)
|
430 |
+
if distance < obstacle_radius:
|
431 |
+
return True # Collision detected
|
432 |
+
return False # No collision
|
433 |
+
|
434 |
+
# Define inverse kinematics with obstacle avoidance
|
435 |
+
def inverse_kinematics(target_position, initial_angles):
|
436 |
+
def error_function(theta):
|
437 |
+
current_position = forward_kinematics(theta)
|
438 |
+
error = np.linalg.norm(target_position - current_position)
|
439 |
+
if is_collision(current_position):
|
440 |
+
error += 1000 # Large penalty for collisions
|
441 |
+
return error
|
442 |
+
|
443 |
+
result = minimize(error_function, initial_angles, method='BFGS')
|
444 |
+
return result.x
|
445 |
+
|
446 |
+
# Generate trajectory (from initial to target position)
|
447 |
+
def generate_trajectory(target_position):
|
448 |
+
initial_angles = [0, 0, 0, 0, 0, 0] # Initial joint angles
|
449 |
+
target_angles = inverse_kinematics(target_position, initial_angles)
|
450 |
+
trajectory = np.linspace(initial_angles, target_angles, 50) # Reduced to 50 points
|
451 |
+
return trajectory
|
452 |
+
|
453 |
+
# Plot the trajectory in 3D with obstacles
|
454 |
+
def plot_trajectory(trajectory):
|
455 |
+
x, y, z = [], [], []
|
456 |
+
for theta in trajectory:
|
457 |
+
position = forward_kinematics(theta)
|
458 |
+
x.append(position[0])
|
459 |
+
y.append(position[1])
|
460 |
+
z.append(position[2])
|
461 |
+
|
462 |
+
fig = plt.figure()
|
463 |
+
ax = fig.add_subplot(111, projection='3d')
|
464 |
+
ax.plot(x, y, z, label="Robot Trajectory")
|
465 |
+
ax.scatter(x[0], y[0], z[0], color='green', label="Start")
|
466 |
+
ax.scatter(x[-1], y[-1], z[-1], color='red', label="Target")
|
467 |
+
|
468 |
+
# Plot obstacles
|
469 |
+
for obstacle in obstacles:
|
470 |
+
obstacle_position = obstacle["position"]
|
471 |
+
obstacle_radius = obstacle["radius"]
|
472 |
+
u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
|
473 |
+
x_obs = obstacle_position[0] + obstacle_radius * np.cos(u) * np.sin(v)
|
474 |
+
y_obs = obstacle_position[1] + obstacle_radius * np.sin(u) * np.sin(v)
|
475 |
+
z_obs = obstacle_position[2] + obstacle_radius * np.cos(v)
|
476 |
+
ax.plot_wireframe(x_obs, y_obs, z_obs, color='orange', alpha=0.5, label="Obstacle" if obstacle == obstacles[0] else "")
|
477 |
+
|
478 |
+
ax.set_xlabel("X")
|
479 |
+
ax.set_ylabel("Y")
|
480 |
+
ax.set_zlabel("Z")
|
481 |
+
ax.legend()
|
482 |
+
return fig
|
483 |
+
|
484 |
+
# Gradio Interface
|
485 |
+
def gradio_interface(x, y, z):
|
486 |
+
target_position = np.array([x, y, z])
|
487 |
+
trajectory = generate_trajectory(target_position)
|
488 |
+
fig = plot_trajectory(trajectory)
|
489 |
+
return fig
|
490 |
+
|
491 |
+
# Launch Gradio App
|
492 |
+
iface = gr.Interface(
|
493 |
+
fn=gradio_interface,
|
494 |
+
inputs=[
|
495 |
+
gr.Number(label="Target X"),
|
496 |
+
gr.Number(label="Target Y"),
|
497 |
+
gr.Number(label="Target Z")
|
498 |
+
],
|
499 |
+
outputs="plot",
|
500 |
+
live=False,
|
501 |
+
title="6-DOF Robot Path Planning with Obstacle Avoidance",
|
502 |
+
description="Enter the target (x, y, z) position and click Submit to generate the optimized trajectory."
|
503 |
+
)
|
504 |
+
|
505 |
+
iface.launch()# app.py
|
506 |
+
import numpy as np
|
507 |
+
import matplotlib.pyplot as plt
|
508 |
+
from mpl_toolkits.mplot3d import Axes3D
|
509 |
+
import gradio as gr
|
510 |
+
from scipy.optimize import minimize
|
511 |
+
|
512 |
+
# Define forward kinematics (simplified example)
|
513 |
+
def forward_kinematics(theta):
|
514 |
+
x = np.cos(theta[0]) + np.cos(theta[1]) + np.cos(theta[2])
|
515 |
+
y = np.sin(theta[0]) + np.sin(theta[1]) + np.sin(theta[2])
|
516 |
+
z = theta[3] + theta[4] + theta[5]
|
517 |
+
return np.array([x, y, z])
|
518 |
+
|
519 |
+
# Define obstacle positions (example)
|
520 |
+
obstacles = [
|
521 |
+
{"position": np.array([0.5, 0.5, 0.5]), "radius": 0.2},
|
522 |
+
{"position": np.array([1.0, 1.0, 1.0]), "radius": 0.3}
|
523 |
+
]
|
524 |
+
|
525 |
+
# Check for collisions with obstacles
|
526 |
+
def is_collision(position):
|
527 |
+
for obstacle in obstacles:
|
528 |
+
obstacle_position = obstacle["position"]
|
529 |
+
obstacle_radius = obstacle["radius"]
|
530 |
+
distance = np.linalg.norm(position - obstacle_position)
|
531 |
+
if distance < obstacle_radius:
|
532 |
+
return True # Collision detected
|
533 |
+
return False # No collision
|
534 |
+
|
535 |
+
# Define inverse kinematics with obstacle avoidance
|
536 |
+
def inverse_kinematics(target_position, initial_angles):
|
537 |
+
def error_function(theta):
|
538 |
+
current_position = forward_kinematics(theta)
|
539 |
+
error = np.linalg.norm(target_position - current_position)
|
540 |
+
if is_collision(current_position):
|
541 |
+
error += 1000 # Large penalty for collisions
|
542 |
+
return error
|
543 |
+
|
544 |
+
result = minimize(error_function, initial_angles, method='BFGS')
|
545 |
+
return result.x
|
546 |
+
|
547 |
+
# Generate trajectory (from initial to target position)
|
548 |
+
def generate_trajectory(target_position):
|
549 |
+
initial_angles = [0, 0, 0, 0, 0, 0] # Initial joint angles
|
550 |
+
target_angles = inverse_kinematics(target_position, initial_angles)
|
551 |
+
trajectory = np.linspace(initial_angles, target_angles, 50) # Reduced to 50 points
|
552 |
+
return trajectory
|
553 |
+
|
554 |
+
# Plot the trajectory in 3D with obstacles
|
555 |
+
def plot_trajectory(trajectory):
|
556 |
+
x, y, z = [], [], []
|
557 |
+
for theta in trajectory:
|
558 |
+
position = forward_kinematics(theta)
|
559 |
+
x.append(position[0])
|
560 |
+
y.append(position[1])
|
561 |
+
z.append(position[2])
|
562 |
+
|
563 |
+
fig = plt.figure()
|
564 |
+
ax = fig.add_subplot(111, projection='3d')
|
565 |
+
ax.plot(x, y, z, label="Robot Trajectory")
|
566 |
+
ax.scatter(x[0], y[0], z[0], color='green', label="Start")
|
567 |
+
ax.scatter(x[-1], y[-1], z[-1], color='red', label="Target")
|
568 |
+
|
569 |
+
# Plot obstacles
|
570 |
+
for obstacle in obstacles:
|
571 |
+
obstacle_position = obstacle["position"]
|
572 |
+
obstacle_radius = obstacle["radius"]
|
573 |
+
u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
|
574 |
+
x_obs = obstacle_position[0] + obstacle_radius * np.cos(u) * np.sin(v)
|
575 |
+
y_obs = obstacle_position[1] + obstacle_radius * np.sin(u) * np.sin(v)
|
576 |
+
z_obs = obstacle_position[2] + obstacle_radius * np.cos(v)
|
577 |
+
ax.plot_wireframe(x_obs, y_obs, z_obs, color='orange', alpha=0.5, label="Obstacle" if obstacle == obstacles[0] else "")
|
578 |
+
|
579 |
+
ax.set_xlabel("X")
|
580 |
+
ax.set_ylabel("Y")
|
581 |
+
ax.set_zlabel("Z")
|
582 |
+
ax.legend()
|
583 |
+
return fig
|
584 |
+
|
585 |
+
# Gradio Interface
|
586 |
+
def gradio_interface(x, y, z):
|
587 |
+
target_position = np.array([x, y, z])
|
588 |
+
trajectory = generate_trajectory(target_position)
|
589 |
+
fig = plot_trajectory(trajectory)
|
590 |
+
return fig
|
591 |
+
|
592 |
+
# Launch Gradio App
|
593 |
+
iface = gr.Interface(
|
594 |
+
fn=gradio_interface,
|
595 |
+
inputs=[
|
596 |
+
gr.Number(label="Target X"),
|
597 |
+
gr.Number(label="Target Y"),
|
598 |
+
gr.Number(label="Target Z")
|
599 |
+
],
|
600 |
+
outputs="plot",
|
601 |
+
live=False,
|
602 |
+
title="6-DOF Robot Path Planning with Obstacle Avoidance",
|
603 |
+
description="Enter the target (x, y, z) position and click Submit to generate the optimized trajectory."
|
604 |
+
)
|
605 |
+
|
606 |
+
iface.launch()# app.py
|
607 |
+
import numpy as np
|
608 |
+
import matplotlib.pyplot as plt
|
609 |
+
from mpl_toolkits.mplot3d import Axes3D
|
610 |
+
import gradio as gr
|
611 |
+
from scipy.optimize import minimize
|
612 |
+
|
613 |
+
# Define forward kinematics (simplified example)
|
614 |
+
def forward_kinematics(theta):
|
615 |
+
x = np.cos(theta[0]) + np.cos(theta[1]) + np.cos(theta[2])
|
616 |
+
y = np.sin(theta[0]) + np.sin(theta[1]) + np.sin(theta[2])
|
617 |
+
z = theta[3] + theta[4] + theta[5]
|
618 |
+
return np.array([x, y, z])
|
619 |
+
|
620 |
+
# Define obstacle positions (example)
|
621 |
+
obstacles = [
|
622 |
+
{"position": np.array([0.5, 0.5, 0.5]), "radius": 0.2},
|
623 |
+
{"position": np.array([1.0, 1.0, 1.0]), "radius": 0.3}
|
624 |
+
]
|
625 |
+
|
626 |
+
# Check for collisions with obstacles
|
627 |
+
def is_collision(position):
|
628 |
+
for obstacle in obstacles:
|
629 |
+
obstacle_position = obstacle["position"]
|
630 |
+
obstacle_radius = obstacle["radius"]
|
631 |
+
distance = np.linalg.norm(position - obstacle_position)
|
632 |
+
if distance < obstacle_radius:
|
633 |
+
return True # Collision detected
|
634 |
+
return False # No collision
|
635 |
+
|
636 |
+
# Define inverse kinematics with obstacle avoidance
|
637 |
+
def inverse_kinematics(target_position, initial_angles):
|
638 |
+
def error_function(theta):
|
639 |
+
current_position = forward_kinematics(theta)
|
640 |
+
error = np.linalg.norm(target_position - current_position)
|
641 |
+
if is_collision(current_position):
|
642 |
+
error += 1000 # Large penalty for collisions
|
643 |
+
return error
|
644 |
+
|
645 |
+
result = minimize(error_function, initial_angles, method='BFGS')
|
646 |
+
return result.x
|
647 |
+
|
648 |
+
# Generate trajectory (from initial to target position)
|
649 |
+
def generate_trajectory(target_position):
|
650 |
+
initial_angles = [0, 0, 0, 0, 0, 0] # Initial joint angles
|
651 |
+
target_angles = inverse_kinematics(target_position, initial_angles)
|
652 |
+
trajectory = np.linspace(initial_angles, target_angles, 50) # Reduced to 50 points
|
653 |
+
return trajectory
|
654 |
+
|
655 |
+
# Plot the trajectory in 3D with obstacles
|
656 |
+
def plot_trajectory(trajectory):
|
657 |
+
x, y, z = [], [], []
|
658 |
+
for theta in trajectory:
|
659 |
+
position = forward_kinematics(theta)
|
660 |
+
x.append(position[0])
|
661 |
+
y.append(position[1])
|
662 |
+
z.append(position[2])
|
663 |
+
|
664 |
+
fig = plt.figure()
|
665 |
+
ax = fig.add_subplot(111, projection='3d')
|
666 |
+
ax.plot(x, y, z, label="Robot Trajectory")
|
667 |
+
ax.scatter(x[0], y[0], z[0], color='green', label="Start")
|
668 |
+
ax.scatter(x[-1], y[-1], z[-1], color='red', label="Target")
|
669 |
+
|
670 |
+
# Plot obstacles
|
671 |
+
for obstacle in obstacles:
|
672 |
+
obstacle_position = obstacle["position"]
|
673 |
+
obstacle_radius = obstacle["radius"]
|
674 |
+
u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
|
675 |
+
x_obs = obstacle_position[0] + obstacle_radius * np.cos(u) * np.sin(v)
|
676 |
+
y_obs = obstacle_position[1] + obstacle_radius * np.sin(u) * np.sin(v)
|
677 |
+
z_obs = obstacle_position[2] + obstacle_radius * np.cos(v)
|
678 |
+
ax.plot_wireframe(x_obs, y_obs, z_obs, color='orange', alpha=0.5, label="Obstacle" if obstacle == obstacles[0] else "")
|
679 |
+
|
680 |
+
ax.set_xlabel("X")
|
681 |
+
ax.set_ylabel("Y")
|
682 |
+
ax.set_zlabel("Z")
|
683 |
+
ax.legend()
|
684 |
+
return fig
|
685 |
+
|
686 |
+
# Gradio Interface
|
687 |
+
def gradio_interface(x, y, z):
|
688 |
+
target_position = np.array([x, y, z])
|
689 |
+
trajectory = generate_trajectory(target_position)
|
690 |
+
fig = plot_trajectory(trajectory)
|
691 |
+
return fig
|
692 |
+
|
693 |
+
# Launch Gradio App
|
694 |
+
iface = gr.Interface(
|
695 |
+
fn=gradio_interface,
|
696 |
+
inputs=[
|
697 |
+
gr.Number(label="Target X"),
|
698 |
+
gr.Number(label="Target Y"),
|
699 |
+
gr.Number(label="Target Z")
|
700 |
+
],
|
701 |
+
outputs="plot",
|
702 |
+
live=False,
|
703 |
+
title="6-DOF Robot Path Planning with Obstacle Avoidance",
|
704 |
+
description="Enter the target (x, y, z) position and click Submit to generate the optimized trajectory."
|
705 |
+
)
|
706 |
+
|
707 |
+
iface.launch()# app.py
|
708 |
+
import numpy as np
|
709 |
+
import matplotlib.pyplot as plt
|
710 |
+
from mpl_toolkits.mplot3d import Axes3D
|
711 |
+
import gradio as gr
|
712 |
+
from scipy.optimize import minimize
|
713 |
+
|
714 |
+
# Define forward kinematics (simplified example)
|
715 |
+
def forward_kinematics(theta):
|
716 |
+
x = np.cos(theta[0]) + np.cos(theta[1]) + np.cos(theta[2])
|
717 |
+
y = np.sin(theta[0]) + np.sin(theta[1]) + np.sin(theta[2])
|
718 |
+
z = theta[3] + theta[4] + theta[5]
|
719 |
+
return np.array([x, y, z])
|
720 |
+
|
721 |
+
# Define obstacle positions (example)
|
722 |
+
obstacles = [
|
723 |
+
{"position": np.array([0.5, 0.5, 0.5]), "radius": 0.2},
|
724 |
+
{"position": np.array([1.0, 1.0, 1.0]), "radius": 0.3}
|
725 |
+
]
|
726 |
+
|
727 |
+
# Check for collisions with obstacles
|
728 |
+
def is_collision(position):
|
729 |
+
for obstacle in obstacles:
|
730 |
+
obstacle_position = obstacle["position"]
|
731 |
+
obstacle_radius = obstacle["radius"]
|
732 |
+
distance = np.linalg.norm(position - obstacle_position)
|
733 |
+
if distance < obstacle_radius:
|
734 |
+
return True # Collision detected
|
735 |
+
return False # No collision
|
736 |
+
|
737 |
+
# Define inverse kinematics with obstacle avoidance
|
738 |
+
def inverse_kinematics(target_position, initial_angles):
|
739 |
+
def error_function(theta):
|
740 |
+
current_position = forward_kinematics(theta)
|
741 |
+
error = np.linalg.norm(target_position - current_position)
|
742 |
+
if is_collision(current_position):
|
743 |
+
error += 1000 # Large penalty for collisions
|
744 |
+
return error
|
745 |
+
|
746 |
+
result = minimize(error_function, initial_angles, method='BFGS')
|
747 |
+
return result.x
|
748 |
+
|
749 |
+
# Generate trajectory (from initial to target position)
|
750 |
+
def generate_trajectory(target_position):
|
751 |
+
initial_angles = [0, 0, 0, 0, 0, 0] # Initial joint angles
|
752 |
+
target_angles = inverse_kinematics(target_position, initial_angles)
|
753 |
+
trajectory = np.linspace(initial_angles, target_angles, 50) # Reduced to 50 points
|
754 |
+
return trajectory
|
755 |
+
|
756 |
+
# Plot the trajectory in 3D with obstacles
|
757 |
+
def plot_trajectory(trajectory):
|
758 |
+
x, y, z = [], [], []
|
759 |
+
for theta in trajectory:
|
760 |
+
position = forward_kinematics(theta)
|
761 |
+
x.append(position[0])
|
762 |
+
y.append(position[1])
|
763 |
+
z.append(position[2])
|
764 |
+
|
765 |
+
fig = plt.figure()
|
766 |
+
ax = fig.add_subplot(111, projection='3d')
|
767 |
+
ax.plot(x, y, z, label="Robot Trajectory")
|
768 |
+
ax.scatter(x[0], y[0], z[0], color='green', label="Start")
|
769 |
+
ax.scatter(x[-1], y[-1], z[-1], color='red', label="Target")
|
770 |
+
|
771 |
+
# Plot obstacles
|
772 |
+
for obstacle in obstacles:
|
773 |
+
obstacle_position = obstacle["position"]
|
774 |
+
obstacle_radius = obstacle["radius"]
|
775 |
+
u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
|
776 |
+
x_obs = obstacle_position[0] + obstacle_radius * np.cos(u) * np.sin(v)
|
777 |
+
y_obs = obstacle_position[1] + obstacle_radius * np.sin(u) * np.sin(v)
|
778 |
+
z_obs = obstacle_position[2] + obstacle_radius * np.cos(v)
|
779 |
+
ax.plot_wireframe(x_obs, y_obs, z_obs, color='orange', alpha=0.5, label="Obstacle" if obstacle == obstacles[0] else "")
|
780 |
+
|
781 |
+
ax.set_xlabel("X")
|
782 |
+
ax.set_ylabel("Y")
|
783 |
+
ax.set_zlabel("Z")
|
784 |
+
ax.legend()
|
785 |
+
return fig
|
786 |
+
|
787 |
+
# Gradio Interface
|
788 |
+
def gradio_interface(x, y, z):
|
789 |
+
target_position = np.array([x, y, z])
|
790 |
+
trajectory = generate_trajectory(target_position)
|
791 |
+
fig = plot_trajectory(trajectory)
|
792 |
+
return fig
|
793 |
+
|
794 |
+
# Launch Gradio App
|
795 |
+
iface = gr.Interface(
|
796 |
+
fn=gradio_interface,
|
797 |
+
inputs=[
|
798 |
+
gr.Number(label="Target X"),
|
799 |
+
gr.Number(label="Target Y"),
|
800 |
+
gr.Number(label="Target Z")
|
801 |
+
],
|
802 |
+
outputs="plot",
|
803 |
+
live=False,
|
804 |
+
title="6-DOF Robot Path Planning with Obstacle Avoidance",
|
805 |
+
description="Enter the target (x, y, z) position and click Submit to generate the optimized trajectory."
|
806 |
+
)
|
807 |
+
|
808 |
+
iface.launch()# app.py
|
809 |
+
import numpy as np
|
810 |
+
import matplotlib.pyplot as plt
|
811 |
+
from mpl_toolkits.mplot3d import Axes3D
|
812 |
+
import gradio as gr
|
813 |
+
from scipy.optimize import minimize
|
814 |
+
|
815 |
+
# Define forward kinematics (simplified example)
|
816 |
+
def forward_kinematics(theta):
|
817 |
+
x = np.cos(theta[0]) + np.cos(theta[1]) + np.cos(theta[2])
|
818 |
+
y = np.sin(theta[0]) + np.sin(theta[1]) + np.sin(theta[2])
|
819 |
+
z = theta[3] + theta[4] + theta[5]
|
820 |
+
return np.array([x, y, z])
|
821 |
+
|
822 |
+
# Define obstacle positions (example)
|
823 |
+
obstacles = [
|
824 |
+
{"position": np.array([0.5, 0.5, 0.5]), "radius": 0.2},
|
825 |
+
{"position": np.array([1.0, 1.0, 1.0]), "radius": 0.3}
|
826 |
+
]
|
827 |
+
|
828 |
+
# Check for collisions with obstacles
|
829 |
+
def is_collision(position):
|
830 |
+
for obstacle in obstacles:
|
831 |
+
obstacle_position = obstacle["position"]
|
832 |
+
obstacle_radius = obstacle["radius"]
|
833 |
+
distance = np.linalg.norm(position - obstacle_position)
|
834 |
+
if distance < obstacle_radius:
|
835 |
+
return True # Collision detected
|
836 |
+
return False # No collision
|
837 |
+
|
838 |
+
# Define inverse kinematics with obstacle avoidance
|
839 |
+
def inverse_kinematics(target_position, initial_angles):
|
840 |
+
def error_function(theta):
|
841 |
+
current_position = forward_kinematics(theta)
|
842 |
+
error = np.linalg.norm(target_position - current_position)
|
843 |
+
if is_collision(current_position):
|
844 |
+
error += 1000 # Large penalty for collisions
|
845 |
+
return error
|
846 |
+
|
847 |
+
result = minimize(error_function, initial_angles, method='BFGS')
|
848 |
+
return result.x
|
849 |
+
|
850 |
+
# Generate trajectory (from initial to target position)
|
851 |
+
def generate_trajectory(target_position):
|
852 |
+
initial_angles = [0, 0, 0, 0, 0, 0] # Initial joint angles
|
853 |
+
target_angles = inverse_kinematics(target_position, initial_angles)
|
854 |
+
trajectory = np.linspace(initial_angles, target_angles, 50) # Reduced to 50 points
|
855 |
+
return trajectory
|
856 |
+
|
857 |
+
# Plot the trajectory in 3D with obstacles
|
858 |
+
def plot_trajectory(trajectory):
|
859 |
+
x, y, z = [], [], []
|
860 |
+
for theta in trajectory:
|
861 |
+
position = forward_kinematics(theta)
|
862 |
+
x.append(position[0])
|
863 |
+
y.append(position[1])
|
864 |
+
z.append(position[2])
|
865 |
+
|
866 |
+
fig = plt.figure()
|
867 |
+
ax = fig.add_subplot(111, projection='3d')
|
868 |
+
ax.plot(x, y, z, label="Robot Trajectory")
|
869 |
+
ax.scatter(x[0], y[0], z[0], color='green', label="Start")
|
870 |
+
ax.scatter(x[-1], y[-1], z[-1], color='red', label="Target")
|
871 |
+
|
872 |
+
# Plot obstacles
|
873 |
+
for obstacle in obstacles:
|
874 |
+
obstacle_position = obstacle["position"]
|
875 |
+
obstacle_radius = obstacle["radius"]
|
876 |
+
u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
|
877 |
+
x_obs = obstacle_position[0] + obstacle_radius * np.cos(u) * np.sin(v)
|
878 |
+
y_obs = obstacle_position[1] + obstacle_radius * np.sin(u) * np.sin(v)
|
879 |
+
z_obs = obstacle_position[2] + obstacle_radius * np.cos(v)
|
880 |
+
ax.plot_wireframe(x_obs, y_obs, z_obs, color='orange', alpha=0.5, label="Obstacle" if obstacle == obstacles[0] else "")
|
881 |
+
|
882 |
+
ax.set_xlabel("X")
|
883 |
+
ax.set_ylabel("Y")
|
884 |
+
ax.set_zlabel("Z")
|
885 |
+
ax.legend()
|
886 |
+
return fig
|
887 |
+
|
888 |
+
# Gradio Interface
|
889 |
+
def gradio_interface(x, y, z):
|
890 |
+
target_position = np.array([x, y, z])
|
891 |
+
trajectory = generate_trajectory(target_position)
|
892 |
+
fig = plot_trajectory(trajectory)
|
893 |
+
return fig
|
894 |
+
|
895 |
+
# Launch Gradio App
|
896 |
+
iface = gr.Interface(
|
897 |
+
fn=gradio_interface,
|
898 |
+
inputs=[
|
899 |
+
gr.Number(label="Target X"),
|
900 |
+
gr.Number(label="Target Y"),
|
901 |
+
gr.Number(label="Target Z")
|
902 |
+
],
|
903 |
+
outputs="plot",
|
904 |
+
live=False,
|
905 |
+
title="6-DOF Robot Path Planning with Obstacle Avoidance",
|
906 |
+
description="Enter the target (x, y, z) position and click Submit to generate the optimized trajectory."
|
907 |
+
)
|
908 |
+
|
909 |
+
iface.launch()# app.py
|
910 |
+
import numpy as np
|
911 |
+
import matplotlib.pyplot as plt
|
912 |
+
from mpl_toolkits.mplot3d import Axes3D
|
913 |
+
import gradio as gr
|
914 |
+
from scipy.optimize import minimize
|
915 |
+
|
916 |
+
# Define forward kinematics (simplified example)
|
917 |
+
def forward_kinematics(theta):
|
918 |
+
x = np.cos(theta[0]) + np.cos(theta[1]) + np.cos(theta[2])
|
919 |
+
y = np.sin(theta[0]) + np.sin(theta[1]) + np.sin(theta[2])
|
920 |
+
z = theta[3] + theta[4] + theta[5]
|
921 |
+
return np.array([x, y, z])
|
922 |
+
|
923 |
+
# Define obstacle positions (example)
|
924 |
+
obstacles = [
|
925 |
+
{"position": np.array([0.5, 0.5, 0.5]), "radius": 0.2},
|
926 |
+
{"position": np.array([1.0, 1.0, 1.0]), "radius": 0.3}
|
927 |
+
]
|
928 |
+
|
929 |
+
# Check for collisions with obstacles
|
930 |
+
def is_collision(position):
|
931 |
+
for obstacle in obstacles:
|
932 |
+
obstacle_position = obstacle["position"]
|
933 |
+
obstacle_radius = obstacle["radius"]
|
934 |
+
distance = np.linalg.norm(position - obstacle_position)
|
935 |
+
if distance < obstacle_radius:
|
936 |
+
return True # Collision detected
|
937 |
+
return False # No collision
|
938 |
+
|
939 |
+
# Define inverse kinematics with obstacle avoidance
|
940 |
+
def inverse_kinematics(target_position, initial_angles):
|
941 |
+
def error_function(theta):
|
942 |
+
current_position = forward_kinematics(theta)
|
943 |
+
error = np.linalg.norm(target_position - current_position)
|
944 |
+
if is_collision(current_position):
|
945 |
+
error += 1000 # Large penalty for collisions
|
946 |
+
return error
|
947 |
+
|
948 |
+
result = minimize(error_function, initial_angles, method='BFGS')
|
949 |
+
return result.x
|
950 |
+
|
951 |
+
# Generate trajectory (from initial to target position)
|
952 |
+
def generate_trajectory(target_position):
|
953 |
+
initial_angles = [0, 0, 0, 0, 0, 0] # Initial joint angles
|
954 |
+
target_angles = inverse_kinematics(target_position, initial_angles)
|
955 |
+
trajectory = np.linspace(initial_angles, target_angles, 50) # Reduced to 50 points
|
956 |
+
return trajectory
|
957 |
+
|
958 |
+
# Plot the trajectory in 3D with obstacles
|
959 |
+
def plot_trajectory(trajectory):
|
960 |
+
x, y, z = [], [], []
|
961 |
+
for theta in trajectory:
|
962 |
+
position = forward_kinematics(theta)
|
963 |
+
x.append(position[0])
|
964 |
+
y.append(position[1])
|
965 |
+
z.append(position[2])
|
966 |
+
|
967 |
+
fig = plt.figure()
|
968 |
+
ax = fig.add_subplot(111, projection='3d')
|
969 |
+
ax.plot(x, y, z, label="Robot Trajectory")
|
970 |
+
ax.scatter(x[0], y[0], z[0], color='green', label="Start")
|
971 |
+
ax.scatter(x[-1], y[-1], z[-1], color='red', label="Target")
|
972 |
+
|
973 |
+
# Plot obstacles
|
974 |
+
for obstacle in obstacles:
|
975 |
+
obstacle_position = obstacle["position"]
|
976 |
+
obstacle_radius = obstacle["radius"]
|
977 |
+
u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
|
978 |
+
x_obs = obstacle_position[0] + obstacle_radius * np.cos(u) * np.sin(v)
|
979 |
+
y_obs = obstacle_position[1] + obstacle_radius * np.sin(u) * np.sin(v)
|
980 |
+
z_obs = obstacle_position[2] + obstacle_radius * np.cos(v)
|
981 |
+
ax.plot_wireframe(x_obs, y_obs, z_obs, color='orange', alpha=0.5, label="Obstacle" if obstacle == obstacles[0] else "")
|
982 |
+
|
983 |
+
ax.set_xlabel("X")
|
984 |
+
ax.set_ylabel("Y")
|
985 |
+
ax.set_zlabel("Z")
|
986 |
+
ax.legend()
|
987 |
+
return fig
|
988 |
+
|
989 |
+
# Gradio Interface
|
990 |
+
def gradio_interface(x, y, z):
|
991 |
+
target_position = np.array([x, y, z])
|
992 |
+
trajectory = generate_trajectory(target_position)
|
993 |
+
fig = plot_trajectory(trajectory)
|
994 |
+
return fig
|
995 |
+
|
996 |
+
# Launch Gradio App
|
997 |
+
iface = gr.Interface(
|
998 |
+
fn=gradio_interface,
|
999 |
+
inputs=[
|
1000 |
+
gr.Number(label="Target X"),
|
1001 |
+
gr.Number(label="Target Y"),
|
1002 |
+
gr.Number(label="Target Z")
|
1003 |
+
],
|
1004 |
+
outputs="plot",
|
1005 |
+
live=False,
|
1006 |
+
title="6-DOF Robot Path Planning with Obstacle Avoidance",
|
1007 |
+
description="Enter the target (x, y, z) position and click Submit to generate the optimized trajectory."
|
1008 |
+
)
|
1009 |
+
|
1010 |
+
iface.launch()# app.py
|
1011 |
+
import numpy as np
|
1012 |
+
import matplotlib.pyplot as plt
|
1013 |
+
from mpl_toolkits.mplot3d import Axes3D
|
1014 |
+
import gradio as gr
|
1015 |
+
from scipy.optimize import minimize
|
1016 |
+
|
1017 |
+
# Define forward kinematics (simplified example)
|
1018 |
+
def forward_kinematics(theta):
|
1019 |
+
x = np.cos(theta[0]) + np.cos(theta[1]) + np.cos(theta[2])
|
1020 |
+
y = np.sin(theta[0]) + np.sin(theta[1]) + np.sin(theta[2])
|
1021 |
+
z = theta[3] + theta[4] + theta[5]
|
1022 |
+
return np.array([x, y, z])
|
1023 |
+
|
1024 |
+
# Define obstacle positions (example)
|
1025 |
+
obstacles = [
|
1026 |
+
{"position": np.array([0.5, 0.5, 0.5]), "radius": 0.2},
|
1027 |
+
{"position": np.array([1.0, 1.0, 1.0]), "radius": 0.3}
|
1028 |
+
]
|
1029 |
+
|
1030 |
+
# Check for collisions with obstacles
|
1031 |
+
def is_collision(position):
|
1032 |
+
for obstacle in obstacles:
|
1033 |
+
obstacle_position = obstacle["position"]
|
1034 |
+
obstacle_radius = obstacle["radius"]
|
1035 |
+
distance = np.linalg.norm(position - obstacle_position)
|
1036 |
+
if distance < obstacle_radius:
|
1037 |
+
return True # Collision detected
|
1038 |
+
return False # No collision
|
1039 |
+
|
1040 |
+
# Define inverse kinematics with obstacle avoidance
|
1041 |
+
def inverse_kinematics(target_position, initial_angles):
|
1042 |
+
def error_function(theta):
|
1043 |
+
current_position = forward_kinematics(theta)
|
1044 |
+
error = np.linalg.norm(target_position - current_position)
|
1045 |
+
if is_collision(current_position):
|
1046 |
+
error += 1000 # Large penalty for collisions
|
1047 |
+
return error
|
1048 |
+
|
1049 |
+
result = minimize(error_function, initial_angles, method='BFGS')
|
1050 |
+
return result.x
|
1051 |
+
|
1052 |
+
# Generate trajectory (from initial to target position)
|
1053 |
+
def generate_trajectory(target_position):
|
1054 |
+
initial_angles = [0, 0, 0, 0, 0, 0] # Initial joint angles
|
1055 |
+
target_angles = inverse_kinematics(target_position, initial_angles)
|
1056 |
+
trajectory = np.linspace(initial_angles, target_angles, 50) # Reduced to 50 points
|
1057 |
+
return trajectory
|
1058 |
+
|
1059 |
+
# Plot the trajectory in 3D with obstacles
|
1060 |
+
def plot_trajectory(trajectory):
|
1061 |
+
x, y, z = [], [], []
|
1062 |
+
for theta in trajectory:
|
1063 |
+
position = forward_kinematics(theta)
|
1064 |
+
x.append(position[0])
|
1065 |
+
y.append(position[1])
|
1066 |
+
z.append(position[2])
|
1067 |
+
|
1068 |
+
fig = plt.figure()
|
1069 |
+
ax = fig.add_subplot(111, projection='3d')
|
1070 |
+
ax.plot(x, y, z, label="Robot Trajectory")
|
1071 |
+
ax.scatter(x[0], y[0], z[0], color='green', label="Start")
|
1072 |
+
ax.scatter(x[-1], y[-1], z[-1], color='red', label="Target")
|
1073 |
+
|
1074 |
+
# Plot obstacles
|
1075 |
+
for obstacle in obstacles:
|
1076 |
+
obstacle_position = obstacle["position"]
|
1077 |
+
obstacle_radius = obstacle["radius"]
|
1078 |
+
u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
|
1079 |
+
x_obs = obstacle_position[0] + obstacle_radius * np.cos(u) * np.sin(v)
|
1080 |
+
y_obs = obstacle_position[1] + obstacle_radius * np.sin(u) * np.sin(v)
|
1081 |
+
z_obs = obstacle_position[2] + obstacle_radius * np.cos(v)
|
1082 |
+
ax.plot_wireframe(x_obs, y_obs, z_obs, color='orange', alpha=0.5, label="Obstacle" if obstacle == obstacles[0] else "")
|
1083 |
+
|
1084 |
+
ax.set_xlabel("X")
|
1085 |
+
ax.set_ylabel("Y")
|
1086 |
+
ax.set_zlabel("Z")
|
1087 |
+
ax.legend()
|
1088 |
+
return fig
|
1089 |
+
|
1090 |
+
# Gradio Interface
|
1091 |
+
def gradio_interface(x, y, z):
|
1092 |
+
target_position = np.array([x, y, z])
|
1093 |
+
trajectory = generate_trajectory(target_position)
|
1094 |
+
fig = plot_trajectory(trajectory)
|
1095 |
+
return fig
|
1096 |
+
|
1097 |
+
# Launch Gradio App
|
1098 |
+
iface = gr.Interface(
|
1099 |
+
fn=gradio_interface,
|
1100 |
+
inputs=[
|
1101 |
+
gr.Number(label="Target X"),
|
1102 |
+
gr.Number(label="Target Y"),
|
1103 |
+
gr.Number(label="Target Z")
|
1104 |
+
],
|
1105 |
+
outputs="plot",
|
1106 |
+
live=False,
|
1107 |
+
title="6-DOF Robot Path Planning with Obstacle Avoidance",
|
1108 |
+
description="Enter the target (x, y, z) position and click Submit to generate the optimized trajectory."
|
1109 |
+
)
|
1110 |
+
|
1111 |
+
iface.launch()# app.py
|
1112 |
+
import numpy as np
|
1113 |
+
import matplotlib.pyplot as plt
|
1114 |
+
from mpl_toolkits.mplot3d import Axes3D
|
1115 |
+
import gradio as gr
|
1116 |
+
from scipy.optimize import minimize
|
1117 |
+
|
1118 |
+
# Define forward kinematics (simplified example)
|
1119 |
+
def forward_kinematics(theta):
|
1120 |
+
x = np.cos(theta[0]) + np.cos(theta[1]) + np.cos(theta[2])
|
1121 |
+
y = np.sin(theta[0]) + np.sin(theta[1]) + np.sin(theta[2])
|
1122 |
+
z = theta[3] + theta[4] + theta[5]
|
1123 |
+
return np.array([x, y, z])
|
1124 |
+
|
1125 |
+
# Define obstacle positions (example)
|
1126 |
+
obstacles = [
|
1127 |
+
{"position": np.array([0.5, 0.5, 0.5]), "radius": 0.2},
|
1128 |
+
{"position": np.array([1.0, 1.0, 1.0]), "radius": 0.3}
|
1129 |
+
]
|
1130 |
+
|
1131 |
+
# Check for collisions with obstacles
|
1132 |
+
def is_collision(position):
|
1133 |
+
for obstacle in obstacles:
|
1134 |
+
obstacle_position = obstacle["position"]
|
1135 |
+
obstacle_radius = obstacle["radius"]
|
1136 |
+
distance = np.linalg.norm(position - obstacle_position)
|
1137 |
+
if distance < obstacle_radius:
|
1138 |
+
return True # Collision detected
|
1139 |
+
return False # No collision
|
1140 |
+
|
1141 |
+
# Define inverse kinematics with obstacle avoidance
|
1142 |
+
def inverse_kinematics(target_position, initial_angles):
|
1143 |
+
def error_function(theta):
|
1144 |
+
current_position = forward_kinematics(theta)
|
1145 |
+
error = np.linalg.norm(target_position - current_position)
|
1146 |
+
if is_collision(current_position):
|
1147 |
+
error += 1000 # Large penalty for collisions
|
1148 |
+
return error
|
1149 |
+
|
1150 |
+
result = minimize(error_function, initial_angles, method='BFGS')
|
1151 |
+
return result.x
|
1152 |
+
|
1153 |
+
# Generate trajectory (from initial to target position)
|
1154 |
+
def generate_trajectory(target_position):
|
1155 |
+
initial_angles = [0, 0, 0, 0, 0, 0] # Initial joint angles
|
1156 |
+
target_angles = inverse_kinematics(target_position, initial_angles)
|
1157 |
+
trajectory = np.linspace(initial_angles, target_angles, 50) # Reduced to 50 points
|
1158 |
+
return trajectory
|
1159 |
+
|
1160 |
+
# Plot the trajectory in 3D with obstacles
|
1161 |
+
def plot_trajectory(trajectory):
|
1162 |
+
x, y, z = [], [], []
|
1163 |
+
for theta in trajectory:
|
1164 |
+
position = forward_kinematics(theta)
|
1165 |
+
x.append(position[0])
|
1166 |
+
y.append(position[1])
|
1167 |
+
z.append(position[2])
|
1168 |
+
|
1169 |
+
fig = plt.figure()
|
1170 |
+
ax = fig.add_subplot(111, projection='3d')
|
1171 |
+
ax.plot(x, y, z, label="Robot Trajectory")
|
1172 |
+
ax.scatter(x[0], y[0], z[0], color='green', label="Start")
|
1173 |
+
ax.scatter(x[-1], y[-1], z[-1], color='red', label="Target")
|
1174 |
+
|
1175 |
+
# Plot obstacles
|
1176 |
+
for obstacle in obstacles:
|
1177 |
+
obstacle_position = obstacle["position"]
|
1178 |
+
obstacle_radius = obstacle["radius"]
|
1179 |
+
u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
|
1180 |
+
x_obs = obstacle_position[0] + obstacle_radius * np.cos(u) * np.sin(v)
|
1181 |
+
y_obs = obstacle_position[1] + obstacle_radius * np.sin(u) * np.sin(v)
|
1182 |
+
z_obs = obstacle_position[2] + obstacle_radius * np.cos(v)
|
1183 |
+
ax.plot_wireframe(x_obs, y_obs, z_obs, color='orange', alpha=0.5, label="Obstacle" if obstacle == obstacles[0] else "")
|
1184 |
+
|
1185 |
+
ax.set_xlabel("X")
|
1186 |
+
ax.set_ylabel("Y")
|
1187 |
+
ax.set_zlabel("Z")
|
1188 |
+
ax.legend()
|
1189 |
+
return fig
|
1190 |
+
|
1191 |
+
# Gradio Interface
|
1192 |
+
def gradio_interface(x, y, z):
|
1193 |
+
target_position = np.array([x, y, z])
|
1194 |
+
trajectory = generate_trajectory(target_position)
|
1195 |
+
fig = plot_trajectory(trajectory)
|
1196 |
+
return fig
|
1197 |
+
|
1198 |
+
# Launch Gradio App
|
1199 |
+
iface = gr.Interface(
|
1200 |
+
fn=gradio_interface,
|
1201 |
+
inputs=[
|
1202 |
+
gr.Number(label="Target X"),
|
1203 |
+
gr.Number(label="Target Y"),
|
1204 |
+
gr.Number(label="Target Z")
|
1205 |
+
],
|
1206 |
+
outputs="plot",
|
1207 |
+
live=False,
|
1208 |
+
title="6-DOF Robot Path Planning with Obstacle Avoidance",
|
1209 |
+
description="Enter the target (x, y, z) position and click Submit to generate the optimized trajectory."
|
1210 |
+
)
|
1211 |
+
|
1212 |
+
iface.launch()# app.py
|
1213 |
+
import numpy as np
|
1214 |
+
import matplotlib.pyplot as plt
|
1215 |
+
from mpl_toolkits.mplot3d import Axes3D
|
1216 |
+
import gradio as gr
|
1217 |
+
from scipy.optimize import minimize
|
1218 |
+
|
1219 |
+
# Define forward kinematics (simplified example)
|
1220 |
+
def forward_kinematics(theta):
|
1221 |
+
x = np.cos(theta[0]) + np.cos(theta[1]) + np.cos(theta[2])
|
1222 |
+
y = np.sin(theta[0]) + np.sin(theta[1]) + np.sin(theta[2])
|
1223 |
+
z = theta[3] + theta[4] + theta[5]
|
1224 |
+
return np.array([x, y, z])
|
1225 |
+
|
1226 |
+
# Define obstacle positions (example)
|
1227 |
+
obstacles = [
|
1228 |
+
{"position": np.array([0.5, 0.5, 0.5]), "radius": 0.2},
|
1229 |
+
{"position": np.array([1.0, 1.0, 1.0]), "radius": 0.3}
|
1230 |
+
]
|
1231 |
+
|
1232 |
+
# Check for collisions with obstacles
|
1233 |
+
def is_collision(position):
|
1234 |
+
for obstacle in obstacles:
|
1235 |
+
obstacle_position = obstacle["position"]
|
1236 |
+
obstacle_radius = obstacle["radius"]
|
1237 |
+
distance = np.linalg.norm(position - obstacle_position)
|
1238 |
+
if distance < obstacle_radius:
|
1239 |
+
return True # Collision detected
|
1240 |
+
return False # No collision
|
1241 |
+
|
1242 |
+
# Define inverse kinematics with obstacle avoidance
|
1243 |
+
def inverse_kinematics(target_position, initial_angles):
|
1244 |
+
def error_function(theta):
|
1245 |
+
current_position = forward_kinematics(theta)
|
1246 |
+
error = np.linalg.norm(target_position - current_position)
|
1247 |
+
if is_collision(current_position):
|
1248 |
+
error += 1000 # Large penalty for collisions
|
1249 |
+
return error
|
1250 |
+
|
1251 |
+
result = minimize(error_function, initial_angles, method='BFGS')
|
1252 |
+
return result.x
|
1253 |
+
|
1254 |
+
# Generate trajectory (from initial to target position)
|
1255 |
+
def generate_trajectory(target_position):
|
1256 |
+
initial_angles = [0, 0, 0, 0, 0, 0] # Initial joint angles
|
1257 |
+
target_angles = inverse_kinematics(target_position, initial_angles)
|
1258 |
+
trajectory = np.linspace(initial_angles, target_angles, 50) # Reduced to 50 points
|
1259 |
+
return trajectory
|
1260 |
+
|
1261 |
+
# Plot the trajectory in 3D with obstacles
|
1262 |
+
def plot_trajectory(trajectory):
|
1263 |
+
x, y, z = [], [], []
|
1264 |
+
for theta in trajectory:
|
1265 |
+
position = forward_kinematics(theta)
|
1266 |
+
x.append(position[0])
|
1267 |
+
y.append(position[1])
|
1268 |
+
z.append(position[2])
|
1269 |
+
|
1270 |
+
fig = plt.figure()
|
1271 |
+
ax = fig.add_subplot(111, projection='3d')
|
1272 |
+
ax.plot(x, y, z, label="Robot Trajectory")
|
1273 |
+
ax.scatter(x[0], y[0], z[0], color='green', label="Start")
|
1274 |
+
ax.scatter(x[-1], y[-1], z[-1], color='red', label="Target")
|
1275 |
+
|
1276 |
+
# Plot obstacles
|
1277 |
+
for obstacle in obstacles:
|
1278 |
+
obstacle_position = obstacle["position"]
|
1279 |
+
obstacle_radius = obstacle["radius"]
|
1280 |
+
u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
|
1281 |
+
x_obs = obstacle_position[0] + obstacle_radius * np.cos(u) * np.sin(v)
|
1282 |
+
y_obs = obstacle_position[1] + obstacle_radius * np.sin(u) * np.sin(v)
|
1283 |
+
z_obs = obstacle_position[2] + obstacle_radius * np.cos(v)
|
1284 |
+
ax.plot_wireframe(x_obs, y_obs, z_obs, color='orange', alpha=0.5, label="Obstacle" if obstacle == obstacles[0] else "")
|
1285 |
+
|
1286 |
+
ax.set_xlabel("X")
|
1287 |
+
ax.set_ylabel("Y")
|
1288 |
+
ax.set_zlabel("Z")
|
1289 |
+
ax.legend()
|
1290 |
+
return fig
|
1291 |
+
|
1292 |
+
# Gradio Interface
|
1293 |
+
def gradio_interface(x, y, z):
|
1294 |
+
target_position = np.array([x, y, z])
|
1295 |
+
trajectory = generate_trajectory(target_position)
|
1296 |
+
fig = plot_trajectory(trajectory)
|
1297 |
+
return fig
|
1298 |
+
|
1299 |
+
# Launch Gradio App
|
1300 |
+
iface = gr.Interface(
|
1301 |
+
fn=gradio_interface,
|
1302 |
+
inputs=[
|
1303 |
+
gr.Number(label="Target X"),
|
1304 |
+
gr.Number(label="Target Y"),
|
1305 |
+
gr.Number(label="Target Z")
|
1306 |
+
],
|
1307 |
+
outputs="plot",
|
1308 |
+
live=False,
|
1309 |
+
title="6-DOF Robot Path Planning with Obstacle Avoidance",
|
1310 |
+
description="Enter the target (x, y, z) position and click Submit to generate the optimized trajectory."
|
1311 |
+
)
|
1312 |
+
|
1313 |
+
iface.launch()# app.py
|
1314 |
+
import numpy as np
|
1315 |
+
import matplotlib.pyplot as plt
|
1316 |
+
from mpl_toolkits.mplot3d import Axes3D
|
1317 |
+
import gradio as gr
|
1318 |
+
from scipy.optimize import minimize
|
1319 |
+
|
1320 |
+
# Define forward kinematics (simplified example)
|
1321 |
+
def forward_kinematics(theta):
|
1322 |
+
x = np.cos(theta[0]) + np.cos(theta[1]) + np.cos(theta[2])
|
1323 |
+
y = np.sin(theta[0]) + np.sin(theta[1]) + np.sin(theta[2])
|
1324 |
+
z = theta[3] + theta[4] + theta[5]
|
1325 |
+
return np.array([x, y, z])
|
1326 |
+
|
1327 |
+
# Define obstacle positions (example)
|
1328 |
+
obstacles = [
|
1329 |
+
{"position": np.array([0.5, 0.5, 0.5]), "radius": 0.2},
|
1330 |
+
{"position": np.array([1.0, 1.0, 1.0]), "radius": 0.3}
|
1331 |
+
]
|
1332 |
+
|
1333 |
+
# Check for collisions with obstacles
|
1334 |
+
def is_collision(position):
|
1335 |
+
for obstacle in obstacles:
|
1336 |
+
obstacle_position = obstacle["position"]
|
1337 |
+
obstacle_radius = obstacle["radius"]
|
1338 |
+
distance = np.linalg.norm(position - obstacle_position)
|
1339 |
+
if distance < obstacle_radius:
|
1340 |
+
return True # Collision detected
|
1341 |
+
return False # No collision
|
1342 |
+
|
1343 |
+
# Define inverse kinematics with obstacle avoidance
|
1344 |
+
def inverse_kinematics(target_position, initial_angles):
|
1345 |
+
def error_function(theta):
|
1346 |
+
current_position = forward_kinematics(theta)
|
1347 |
+
error = np.linalg.norm(target_position - current_position)
|
1348 |
+
if is_collision(current_position):
|
1349 |
+
error += 1000 # Large penalty for collisions
|
1350 |
+
return error
|
1351 |
+
|
1352 |
+
result = minimize(error_function, initial_angles, method='BFGS')
|
1353 |
+
return result.x
|
1354 |
+
|
1355 |
+
# Generate trajectory (from initial to target position)
|
1356 |
+
def generate_trajectory(target_position):
|
1357 |
+
initial_angles = [0, 0, 0, 0, 0, 0] # Initial joint angles
|
1358 |
+
target_angles = inverse_kinematics(target_position, initial_angles)
|
1359 |
+
trajectory = np.linspace(initial_angles, target_angles, 50) # Reduced to 50 points
|
1360 |
+
return trajectory
|
1361 |
+
|
1362 |
+
# Plot the trajectory in 3D with obstacles
|
1363 |
+
def plot_trajectory(trajectory):
|
1364 |
+
x, y, z = [], [], []
|
1365 |
+
for theta in trajectory:
|
1366 |
+
position = forward_kinematics(theta)
|
1367 |
+
x.append(position[0])
|
1368 |
+
y.append(position[1])
|
1369 |
+
z.append(position[2])
|
1370 |
+
|
1371 |
+
fig = plt.figure()
|
1372 |
+
ax = fig.add_subplot(111, projection='3d')
|
1373 |
+
ax.plot(x, y, z, label="Robot Trajectory")
|
1374 |
+
ax.scatter(x[0], y[0], z[0], color='green', label="Start")
|
1375 |
+
ax.scatter(x[-1], y[-1], z[-1], color='red', label="Target")
|
1376 |
+
|
1377 |
+
# Plot obstacles
|
1378 |
+
for obstacle in obstacles:
|
1379 |
+
obstacle_position = obstacle["position"]
|
1380 |
+
obstacle_radius = obstacle["radius"]
|
1381 |
+
u, v = np.mgrid[0:2*np.pi:20j, 0:np.pi:10j]
|
1382 |
+
x_obs = obstacle_position[0] + obstacle_radius * np.cos(u) * np.sin(v)
|
1383 |
+
y_obs = obstacle_position[1] + obstacle_radius * np.sin(u) * np.sin(v)
|
1384 |
+
z_obs = obstacle_position[2] + obstacle_radius * np.cos(v)
|
1385 |
+
ax.plot_wireframe(x_obs, y_obs, z_obs, color='orange', alpha=0.5, label="Obstacle" if obstacle == obstacles[0] else "")
|
1386 |
+
|
1387 |
+
ax.set_xlabel("X")
|
1388 |
+
ax.set_ylabel("Y")
|
1389 |
+
ax.set_zlabel("Z")
|
1390 |
+
ax.legend()
|
1391 |
+
return fig
|
1392 |
+
|
1393 |
+
# Gradio Interface
|
1394 |
+
def gradio_interface(x, y, z):
|
1395 |
+
target_position = np.array([x, y, z])
|
1396 |
+
trajectory = generate_trajectory(target_position)
|
1397 |
+
fig = plot_trajectory(trajectory)
|
1398 |
+
return fig
|
1399 |
+
|
1400 |
+
# Launch Gradio App
|
1401 |
+
iface = gr.Interface(
|
1402 |
+
fn=gradio_interface,
|
1403 |
+
inputs=[
|
1404 |
+
gr.Number(label="Target X"),
|
1405 |
+
gr.Number(label="Target Y"),
|
1406 |
+
gr.Number(label="Target Z")
|
1407 |
+
],
|
1408 |
+
outputs="plot",
|
1409 |
+
live=False,
|
1410 |
+
title="6-DOF Robot Path Planning with Obstacle Avoidance",
|
1411 |
+
description="Enter the target (x, y, z) position and click Submit to generate the optimized trajectory."
|
1412 |
+
)
|
1413 |
+
|
1414 |
+
iface.launch()
|