š¬ Amaliyot: Robot Kinematikasi Simulyatsiyasi
Laboratoriya maqsadiā
Bu laboratoriyada 2-DOF va 3-DOF robot qo'llarining kinematikasini simulyatsiya qilamiz. Forward va inverse kinematikani vizual tarzda o'rganamiz.
Kerakli kutubxonalarā
pip install numpy matplotlib
1-laboratoriya: 2-DOF Robot Qo'liā
To'liq simulyatsiyaā
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.widgets import Slider
from matplotlib.patches import Circle, FancyArrowPatch
class RobotArm2DOF:
"""2 bo'g'inli robot qo'li simulyatsiyasi"""
def __init__(self, L1=1.0, L2=0.8):
self.L1 = L1 # 1-bo'g'in uzunligi
self.L2 = L2 # 2-bo'g'in uzunligi
self.theta1 = 0 # 1-bo'g'in burchagi (rad)
self.theta2 = 0 # 2-bo'g'in burchagi (rad)
def forward_kinematics(self, theta1, theta2):
"""Forward kinematika: burchaklardan pozitsiyaga"""
# 1-bo'g'in uchi
x1 = self.L1 * np.cos(theta1)
y1 = self.L1 * np.sin(theta1)
# 2-bo'g'in uchi (end effector)
x2 = x1 + self.L2 * np.cos(theta1 + theta2)
y2 = y1 + self.L2 * np.sin(theta1 + theta2)
return (x1, y1), (x2, y2)
def inverse_kinematics(self, x, y):
"""Inverse kinematika: pozitsiyadan burchaklarga"""
# Maqsad nuqtaga masofa
d = np.sqrt(x**2 + y**2)
# Yetib bo'ladimi tekshirish
if d > self.L1 + self.L2:
print(f"Xato: Nuqta ({x:.2f}, {y:.2f}) juda uzoq!")
return None, None
if d < abs(self.L1 - self.L2):
print(f"Xato: Nuqta ({x:.2f}, {y:.2f}) juda yaqin!")
return None, None
# Kosinus teoremasi yordamida
cos_theta2 = (x**2 + y**2 - self.L1**2 - self.L2**2) / (2 * self.L1 * self.L2)
cos_theta2 = np.clip(cos_theta2, -1, 1) # Raqamli xatolarni oldini olish
# Elbow up yechimi
theta2 = np.arccos(cos_theta2)
# Theta1 ni hisoblash
k1 = self.L1 + self.L2 * np.cos(theta2)
k2 = self.L2 * np.sin(theta2)
theta1 = np.arctan2(y, x) - np.arctan2(k2, k1)
return theta1, theta2
def workspace_analysis(self, resolution=100):
"""Ish maydonini tahlil qilish"""
theta1_range = np.linspace(-np.pi, np.pi, resolution)
theta2_range = np.linspace(-np.pi, np.pi, resolution)
x_points = []
y_points = []
for t1 in theta1_range:
for t2 in theta2_range:
_, (x, y) = self.forward_kinematics(t1, t2)
x_points.append(x)
y_points.append(y)
return np.array(x_points), np.array(y_points)
def jacobian(self, theta1, theta2):
"""Jacobian matritsasini hisoblash"""
J = np.array([
[-self.L1*np.sin(theta1) - self.L2*np.sin(theta1+theta2), -self.L2*np.sin(theta1+theta2)],
[self.L1*np.cos(theta1) + self.L2*np.cos(theta1+theta2), self.L2*np.cos(theta1+theta2)]
])
return J
def manipulability(self, theta1, theta2):
"""Manipulyatsiya indeksini hisoblash"""
J = self.jacobian(theta1, theta2)
return np.sqrt(np.linalg.det(J @ J.T))
def interactive_simulation():
"""Interaktiv robot simulyatsiyasi"""
robot = RobotArm2DOF(L1=1.0, L2=0.8)
fig, axes = plt.subplots(1, 2, figsize=(14, 6))
plt.subplots_adjust(bottom=0.25)
# Robot vizualizatsiyasi
ax1 = axes[0]
ax1.set_xlim(-2.5, 2.5)
ax1.set_ylim(-2.5, 2.5)
ax1.set_aspect('equal')
ax1.grid(True, alpha=0.3)
ax1.set_title('2-DOF Robot Qo\'li', fontsize=14)
ax1.set_xlabel('X (m)')
ax1.set_ylabel('Y (m)')
# Ish maydoni
wx, wy = robot.workspace_analysis(50)
ax1.scatter(wx, wy, c='lightblue', s=1, alpha=0.3, label='Ish maydoni')
# Boshlang'ich holat
(x1, y1), (x2, y2) = robot.forward_kinematics(0.5, 0.5)
# Robot chizish
base = Circle((0, 0), 0.08, color='darkblue', zorder=5)
ax1.add_patch(base)
link1, = ax1.plot([0, x1], [0, y1], 'b-', linewidth=8, solid_capstyle='round')
link2, = ax1.plot([x1, x2], [y1, y2], 'r-', linewidth=6, solid_capstyle='round')
joint1 = Circle((x1, y1), 0.06, color='purple', zorder=5)
ax1.add_patch(joint1)
end_effector, = ax1.plot(x2, y2, 'go', markersize=15, label='End effector')
# Trajectory
trajectory_x, trajectory_y = [], []
trajectory_line, = ax1.plot([], [], 'g--', alpha=0.5, linewidth=1)
# Pozitsiya va manipulability ko'rsatish
pos_text = ax1.text(-2.3, 2.0, '', fontsize=10)
manip_text = ax1.text(-2.3, 1.7, '', fontsize=10)
# Jacobian ellips
ax2 = axes[1]
ax2.set_xlim(-3, 3)
ax2.set_ylim(-3, 3)
ax2.set_aspect('equal')
ax2.grid(True, alpha=0.3)
ax2.set_title('Jacobian Tezlik Ellipsi', fontsize=14)
ax2.set_xlabel('Vx (m/s)')
ax2.set_ylabel('Vy (m/s)')
ellipse_line, = ax2.plot([], [], 'b-', linewidth=2)
# Sliderlar
ax_theta1 = plt.axes([0.2, 0.15, 0.6, 0.03])
ax_theta2 = plt.axes([0.2, 0.10, 0.6, 0.03])
slider_theta1 = Slider(ax_theta1, 'Īøā (Ā°)', -180, 180, valinit=np.degrees(0.5))
slider_theta2 = Slider(ax_theta2, 'Īøā (Ā°)', -180, 180, valinit=np.degrees(0.5))
def update(val):
theta1 = np.radians(slider_theta1.val)
theta2 = np.radians(slider_theta2.val)
(x1, y1), (x2, y2) = robot.forward_kinematics(theta1, theta2)
# Robot yangilash
link1.set_data([0, x1], [0, y1])
link2.set_data([x1, x2], [y1, y2])
joint1.center = (x1, y1)
end_effector.set_data([x2], [y2])
# Trajectory
trajectory_x.append(x2)
trajectory_y.append(y2)
if len(trajectory_x) > 200:
trajectory_x.pop(0)
trajectory_y.pop(0)
trajectory_line.set_data(trajectory_x, trajectory_y)
# Matn yangilash
pos_text.set_text(f'Pozitsiya: ({x2:.3f}, {y2:.3f}) m')
# Manipulability
m = robot.manipulability(theta1, theta2)
manip_text.set_text(f'Manipulability: {m:.3f}')
# Jacobian ellips
J = robot.jacobian(theta1, theta2)
U, S, Vt = np.linalg.svd(J)
# Ellips parametrlari
theta_ellipse = np.linspace(0, 2*np.pi, 100)
ellipse_x = S[0] * np.cos(theta_ellipse)
ellipse_y = S[1] * np.sin(theta_ellipse)
# Burilish
angle = np.arctan2(U[1, 0], U[0, 0])
rotation = np.array([[np.cos(angle), -np.sin(angle)],
[np.sin(angle), np.cos(angle)]])
ellipse_points = rotation @ np.vstack([ellipse_x, ellipse_y])
ellipse_line.set_data(ellipse_points[0], ellipse_points[1])
fig.canvas.draw_idle()
slider_theta1.on_changed(update)
slider_theta2.on_changed(update)
update(None)
ax1.legend(loc='upper right')
plt.show()
if __name__ == "__main__":
print("="*50)
print("2-DOF ROBOT QO'LI SIMULYATSIYASI")
print("="*50)
interactive_simulation()
2-laboratoriya: Inverse Kinematika Vizualizatsiyasiā
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.widgets import Button
class InverseKinematicsDemo:
"""Inverse kinematika vizual demonstratsiyasi"""
def __init__(self):
self.L1 = 1.0
self.L2 = 0.8
self.target_x = 1.0
self.target_y = 0.5
def solve_ik(self, x, y, elbow_up=True):
"""IK yechimi"""
d = np.sqrt(x**2 + y**2)
if d > self.L1 + self.L2 or d < abs(self.L1 - self.L2):
return None, None
cos_theta2 = (x**2 + y**2 - self.L1**2 - self.L2**2) / (2 * self.L1 * self.L2)
cos_theta2 = np.clip(cos_theta2, -1, 1)
if elbow_up:
theta2 = np.arccos(cos_theta2)
else:
theta2 = -np.arccos(cos_theta2)
k1 = self.L1 + self.L2 * np.cos(theta2)
k2 = self.L2 * np.sin(theta2)
theta1 = np.arctan2(y, x) - np.arctan2(k2, k1)
return theta1, theta2
def get_positions(self, theta1, theta2):
"""Bo'g'in pozitsiyalarini olish"""
x1 = self.L1 * np.cos(theta1)
y1 = self.L1 * np.sin(theta1)
x2 = x1 + self.L2 * np.cos(theta1 + theta2)
y2 = y1 + self.L2 * np.sin(theta1 + theta2)
return x1, y1, x2, y2
def run(self):
"""Interaktiv demo"""
fig, ax = plt.subplots(figsize=(10, 10))
ax.set_xlim(-2.5, 2.5)
ax.set_ylim(-2.5, 2.5)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.set_title('Inverse Kinematika: Click to set target', fontsize=14)
# Ish maydoni chegarasi
theta = np.linspace(0, 2*np.pi, 100)
outer_x = (self.L1 + self.L2) * np.cos(theta)
outer_y = (self.L1 + self.L2) * np.sin(theta)
inner_x = abs(self.L1 - self.L2) * np.cos(theta)
inner_y = abs(self.L1 - self.L2) * np.sin(theta)
ax.plot(outer_x, outer_y, 'g--', alpha=0.5, label='Tashqi chegara')
ax.plot(inner_x, inner_y, 'r--', alpha=0.5, label='Ichki chegara')
ax.fill_between(outer_x, outer_y, alpha=0.1, color='green')
# Robot (elbow up)
link1_up, = ax.plot([], [], 'b-', linewidth=6, label='Elbow Up')
link2_up, = ax.plot([], [], 'b-', linewidth=4)
# Robot (elbow down)
link1_down, = ax.plot([], [], 'r-', linewidth=6, alpha=0.5, label='Elbow Down')
link2_down, = ax.plot([], [], 'r-', linewidth=4, alpha=0.5)
# Target
target_point, = ax.plot([], [], 'g*', markersize=20, label='Target')
# Info text
info_text = ax.text(-2.3, 2.0, '', fontsize=10, family='monospace')
def update_robot(x, y):
# Elbow up yechimi
t1_up, t2_up = self.solve_ik(x, y, elbow_up=True)
if t1_up is not None:
x1, y1, x2, y2 = self.get_positions(t1_up, t2_up)
link1_up.set_data([0, x1], [0, y1])
link2_up.set_data([x1, x2], [y1, y2])
# Elbow down yechimi
t1_down, t2_down = self.solve_ik(x, y, elbow_up=False)
if t1_down is not None:
x1, y1, x2, y2 = self.get_positions(t1_down, t2_down)
link1_down.set_data([0, x1], [0, y1])
link2_down.set_data([x1, x2], [y1, y2])
target_point.set_data([x], [y])
# Info
info = f"Target: ({x:.2f}, {y:.2f})\n"
if t1_up is not None:
info += f"Elbow Up: Īøā={np.degrees(t1_up):6.1f}Ā°, Īøā={np.degrees(t2_up):6.1f}Ā°\n"
if t1_down is not None:
info += f"Elbow Down: Īøā={np.degrees(t1_down):6.1f}Ā°, Īøā={np.degrees(t2_down):6.1f}Ā°"
info_text.set_text(info)
fig.canvas.draw_idle()
def on_click(event):
if event.inaxes == ax:
update_robot(event.xdata, event.ydata)
fig.canvas.mpl_connect('button_press_event', on_click)
# Boshlang'ich holat
update_robot(1.0, 0.5)
ax.legend(loc='lower right')
plt.show()
if __name__ == "__main__":
demo = InverseKinematicsDemo()
demo.run()
3-laboratoriya: 3-DOF Robot Qo'liā
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
class Robot3DOF:
"""3 bo'g'inli planar robot"""
def __init__(self, L1=1.0, L2=0.8, L3=0.5):
self.L1 = L1
self.L2 = L2
self.L3 = L3
def forward_kinematics(self, theta1, theta2, theta3):
"""Forward kinematika"""
# 1-bo'g'in
x1 = self.L1 * np.cos(theta1)
y1 = self.L1 * np.sin(theta1)
# 2-bo'g'in
x2 = x1 + self.L2 * np.cos(theta1 + theta2)
y2 = y1 + self.L2 * np.sin(theta1 + theta2)
# 3-bo'g'in (end effector)
x3 = x2 + self.L3 * np.cos(theta1 + theta2 + theta3)
y3 = y2 + self.L3 * np.sin(theta1 + theta2 + theta3)
return [(0, 0), (x1, y1), (x2, y2), (x3, y3)]
def inverse_kinematics(self, x, y, phi):
"""
Inverse kinematika
phi - end effector orientatsiyasi (rad)
"""
# Wrist pozitsiyasi
wx = x - self.L3 * np.cos(phi)
wy = y - self.L3 * np.sin(phi)
# Wrist uchun 2-DOF IK
d = np.sqrt(wx**2 + wy**2)
if d > self.L1 + self.L2:
return None
cos_theta2 = (wx**2 + wy**2 - self.L1**2 - self.L2**2) / (2 * self.L1 * self.L2)
cos_theta2 = np.clip(cos_theta2, -1, 1)
theta2 = np.arccos(cos_theta2)
k1 = self.L1 + self.L2 * np.cos(theta2)
k2 = self.L2 * np.sin(theta2)
theta1 = np.arctan2(wy, wx) - np.arctan2(k2, k1)
# Theta3
theta3 = phi - theta1 - theta2
return theta1, theta2, theta3
def jacobian(self, theta1, theta2, theta3):
"""3x3 Jacobian"""
s1 = np.sin(theta1)
s12 = np.sin(theta1 + theta2)
s123 = np.sin(theta1 + theta2 + theta3)
c1 = np.cos(theta1)
c12 = np.cos(theta1 + theta2)
c123 = np.cos(theta1 + theta2 + theta3)
J = np.array([
[-self.L1*s1 - self.L2*s12 - self.L3*s123, -self.L2*s12 - self.L3*s123, -self.L3*s123],
[self.L1*c1 + self.L2*c12 + self.L3*c123, self.L2*c12 + self.L3*c123, self.L3*c123],
[1, 1, 1]
])
return J
def animate_3dof_circle():
"""3-DOF robot bilan aylana chizish"""
robot = Robot3DOF()
fig, ax = plt.subplots(figsize=(10, 10))
ax.set_xlim(-3, 3)
ax.set_ylim(-3, 3)
ax.set_aspect('equal')
ax.grid(True, alpha=0.3)
ax.set_title('3-DOF Robot: Aylana Chizish', fontsize=14)
# Maqsad aylana
t = np.linspace(0, 2*np.pi, 100)
circle_x = 1.2 + 0.5 * np.cos(t)
circle_y = 0.5 + 0.5 * np.sin(t)
ax.plot(circle_x, circle_y, 'g--', alpha=0.5, label='Maqsad trajectory')
# Robot
link1, = ax.plot([], [], 'b-', linewidth=8)
link2, = ax.plot([], [], 'r-', linewidth=6)
link3, = ax.plot([], [], 'm-', linewidth=4)
joints, = ax.plot([], [], 'ko', markersize=10)
end_effector, = ax.plot([], [], 'g*', markersize=15)
# Trajectory
traj_x, traj_y = [], []
trajectory, = ax.plot([], [], 'g-', alpha=0.5, linewidth=2)
def init():
return link1, link2, link3, joints, end_effector, trajectory
def animate(i):
t = i * 0.1
x = 1.2 + 0.5 * np.cos(t)
y = 0.5 + 0.5 * np.sin(t)
phi = t # Orientatsiya ham aylanadi
result = robot.inverse_kinematics(x, y, phi)
if result is None:
return link1, link2, link3, joints, end_effector, trajectory
theta1, theta2, theta3 = result
points = robot.forward_kinematics(theta1, theta2, theta3)
# Robot chizish
link1.set_data([points[0][0], points[1][0]], [points[0][1], points[1][1]])
link2.set_data([points[1][0], points[2][0]], [points[1][1], points[2][1]])
link3.set_data([points[2][0], points[3][0]], [points[2][1], points[3][1]])
joints.set_data([p[0] for p in points[:-1]], [p[1] for p in points[:-1]])
end_effector.set_data([points[3][0]], [points[3][1]])
traj_x.append(points[3][0])
traj_y.append(points[3][1])
trajectory.set_data(traj_x, traj_y)
return link1, link2, link3, joints, end_effector, trajectory
from matplotlib.animation import FuncAnimation
anim = FuncAnimation(fig, animate, init_func=init, frames=200,
interval=50, blit=True)
ax.legend()
plt.show()
return anim
if __name__ == "__main__":
anim = animate_3dof_circle()
4-laboratoriya: Trajectory Planningā
import numpy as np
import matplotlib.pyplot as plt
class TrajectoryPlanner:
"""Trajectory rejalashtirish"""
@staticmethod
def linear_interpolation(start, end, steps):
"""Chiziqli interpolyatsiya"""
return np.linspace(start, end, steps)
@staticmethod
def cubic_polynomial(q0, qf, t0, tf, steps):
"""
Kubik polinom trajectory
q(t) = a0 + a1*t + a2*tĀ² + a3*tĀ³
Chegaraviy shartlar: q(t0)=q0, q(tf)=qf, q'(t0)=0, q'(tf)=0
"""
T = tf - t0
# Koeffitsientlar
a0 = q0
a1 = 0
a2 = 3 * (qf - q0) / T**2
a3 = -2 * (qf - q0) / T**3
t = np.linspace(0, T, steps)
q = a0 + a1*t + a2*t**2 + a3*t**3
v = a1 + 2*a2*t + 3*a3*t**2
a = 2*a2 + 6*a3*t
return t + t0, q, v, a
@staticmethod
def quintic_polynomial(q0, qf, v0, vf, a0_val, af, t0, tf, steps):
"""
5-darajali polinom trajectory
q(t) = a0 + a1*t + a2*tĀ² + a3*tĀ³ + a4*tā“ + a5*tāµ
"""
T = tf - t0
# Matritsa tenglamasi
M = np.array([
[1, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 0, 2, 0, 0, 0],
[1, T, T**2, T**3, T**4, T**5],
[0, 1, 2*T, 3*T**2, 4*T**3, 5*T**4],
[0, 0, 2, 6*T, 12*T**2, 20*T**3]
])
b = np.array([q0, v0, a0_val, qf, vf, af])
coeffs = np.linalg.solve(M, b)
t = np.linspace(0, T, steps)
q = np.polyval(coeffs[::-1], t)
# Tezlik
v_coeffs = np.array([coeffs[1], 2*coeffs[2], 3*coeffs[3], 4*coeffs[4], 5*coeffs[5]])
v = np.polyval(v_coeffs[::-1], t)
# Tezlanish
a_coeffs = np.array([2*coeffs[2], 6*coeffs[3], 12*coeffs[4], 20*coeffs[5]])
a = np.polyval(a_coeffs[::-1], t)
return t + t0, q, v, a
@staticmethod
def trapezoidal_velocity(q0, qf, v_max, a_max, dt=0.01):
"""
Trapetsoid tezlik profili
"""
d = abs(qf - q0)
sign = np.sign(qf - q0)
# Tezlanish vaqti
t_acc = v_max / a_max
# Tezlanish masofasi
d_acc = 0.5 * a_max * t_acc**2
if 2 * d_acc > d:
# Uchburchak profil
t_acc = np.sqrt(d / a_max)
t_cruise = 0
v_max = a_max * t_acc
else:
# Trapetsoid profil
d_cruise = d - 2 * d_acc
t_cruise = d_cruise / v_max
t_total = 2 * t_acc + t_cruise
t = np.arange(0, t_total + dt, dt)
q = np.zeros_like(t)
v = np.zeros_like(t)
a = np.zeros_like(t)
for i, ti in enumerate(t):
if ti < t_acc:
# Tezlanish fazasi
a[i] = a_max
v[i] = a_max * ti
q[i] = 0.5 * a_max * ti**2
elif ti < t_acc + t_cruise:
# Doimiy tezlik
a[i] = 0
v[i] = v_max
q[i] = d_acc + v_max * (ti - t_acc)
else:
# Sekinlashish
t_dec = ti - t_acc - t_cruise
a[i] = -a_max
v[i] = v_max - a_max * t_dec
q[i] = d_acc + v_max * t_cruise + v_max * t_dec - 0.5 * a_max * t_dec**2
return t, q0 + sign * q, sign * v, sign * a
def compare_trajectories():
"""Turli trajectory usullarini solishtirish"""
planner = TrajectoryPlanner()
q0, qf = 0, 90 # Burchak (gradus)
fig, axes = plt.subplots(3, 1, figsize=(12, 10))
# 1. Kubik polinom
t1, q1, v1, a1 = planner.cubic_polynomial(q0, qf, 0, 2, 200)
# 2. 5-darajali polinom
t2, q2, v2, a2 = planner.quintic_polynomial(q0, qf, 0, 0, 0, 0, 0, 2, 200)
# 3. Trapetsoid
t3, q3, v3, a3 = planner.trapezoidal_velocity(q0, qf, 60, 100, 0.01)
# Pozitsiya
axes[0].plot(t1, q1, 'b-', label='Kubik', linewidth=2)
axes[0].plot(t2, q2, 'r--', label='Kvintik', linewidth=2)
axes[0].plot(t3, q3, 'g:', label='Trapetsoid', linewidth=2)
axes[0].set_ylabel('Pozitsiya (Ā°)', fontsize=12)
axes[0].set_title('Trajectory Rejalashtirish Usullari', fontsize=14)
axes[0].legend()
axes[0].grid(True, alpha=0.3)
# Tezlik
axes[1].plot(t1, v1, 'b-', linewidth=2)
axes[1].plot(t2, v2, 'r--', linewidth=2)
axes[1].plot(t3, v3, 'g:', linewidth=2)
axes[1].set_ylabel('Tezlik (Ā°/s)', fontsize=12)
axes[1].grid(True, alpha=0.3)
# Tezlanish
axes[2].plot(t1, a1, 'b-', linewidth=2)
axes[2].plot(t2, a2, 'r--', linewidth=2)
axes[2].plot(t3, a3, 'g:', linewidth=2)
axes[2].set_ylabel('Tezlanish (Ā°/sĀ²)', fontsize=12)
axes[2].set_xlabel('Vaqt (s)', fontsize=12)
axes[2].grid(True, alpha=0.3)
plt.tight_layout()
plt.show()
if __name__ == "__main__":
compare_trajectories()
5-laboratoriya: DH Parametrlariā
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def dh_transform(a, alpha, d, theta):
"""
Denavit-Hartenberg transformatsiya matritsasi
a - link uzunligi (x o'qi bo'ylab)
alpha - link burilishi (x o'qi atrofida)
d - offset (z o'qi bo'ylab)
theta - joint burchagi (z o'qi atrofida)
"""
ct = np.cos(theta)
st = np.sin(theta)
ca = np.cos(alpha)
sa = np.sin(alpha)
T = np.array([
[ct, -st*ca, st*sa, a*ct],
[st, ct*ca, -ct*sa, a*st],
[0, sa, ca, d],
[0, 0, 0, 1]
])
return T
def robot_arm_3d_simulation():
"""3D robot qo'li DH parametrlari bilan"""
# PUMA 560 ga o'xshash 6-DOF robot DH parametrlari
# (soddalashtirilgan)
dh_params = [
# a, alpha, d, theta (variable)
[0, np.pi/2, 0.5, 0], # Joint 1
[0.5, 0, 0, 0], # Joint 2
[0, np.pi/2, 0, 0], # Joint 3
[0, -np.pi/2, 0.4, 0], # Joint 4
[0, np.pi/2, 0, 0], # Joint 5
[0, 0, 0.1, 0], # Joint 6
]
def forward_kinematics(joint_angles):
"""Forward kinematika DH usuli bilan"""
T = np.eye(4)
positions = [(0, 0, 0)]
for i, (a, alpha, d, _) in enumerate(dh_params):
theta = joint_angles[i]
T = T @ dh_transform(a, alpha, d, theta)
positions.append((T[0, 3], T[1, 3], T[2, 3]))
return positions, T
# Interaktiv vizualizatsiya
fig = plt.figure(figsize=(12, 8))
ax = fig.add_subplot(111, projection='3d')
# Boshlang'ich burchaklar
angles = [0, np.pi/4, -np.pi/4, 0, np.pi/4, 0]
positions, T_end = forward_kinematics(angles)
# Robot chizish
xs = [p[0] for p in positions]
ys = [p[1] for p in positions]
zs = [p[2] for p in positions]
ax.plot(xs, ys, zs, 'b-', linewidth=4, label='Robot links')
ax.scatter(xs, ys, zs, c='red', s=100, label='Joints')
ax.scatter(xs[-1], ys[-1], zs[-1], c='green', s=200, marker='*', label='End effector')
# Koordinata tizimlari
for i, pos in enumerate(positions):
ax.quiver(pos[0], pos[1], pos[2], 0.1, 0, 0, color='r', arrow_length_ratio=0.3)
ax.quiver(pos[0], pos[1], pos[2], 0, 0.1, 0, color='g', arrow_length_ratio=0.3)
ax.quiver(pos[0], pos[1], pos[2], 0, 0, 0.1, color='b', arrow_length_ratio=0.3)
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.set_title('6-DOF Robot (DH Parametrlari)', fontsize=14)
ax.legend()
# Bir xil masshtab
max_range = 1.5
ax.set_xlim([-max_range, max_range])
ax.set_ylim([-max_range, max_range])
ax.set_zlim([0, max_range * 1.5])
# End effector pozitsiyasi
print(f"End effector pozitsiyasi: ({xs[-1]:.3f}, {ys[-1]:.3f}, {zs[-1]:.3f})")
print(f"\nTransformatsiya matritsasi:\n{T_end}")
plt.show()
if __name__ == "__main__":
robot_arm_3d_simulation()
Xulosaā
Bu laboratoriyalarda biz:
- ā 2-DOF va 3-DOF robot qo'llarini simulyatsiya qildik
- ā Forward va Inverse kinematikani vizualizatsiya qildik
- ā Jacobian va manipulability ni o'rgandik
- ā Trajectory planning usullarini soldik
- ā DH parametrlari bilan 3D robot yaratdik
Keyingi qadamlarā
- Real robotlar bilan ishlash (Arduino/ROS)
- Motion planning algoritmlari (RRT, A*)
- Dynamic control