6.1 Robot Kinematikasi — Masalalar

Jami: 25 ta | Yechim bilan: ✅

---

Asosiy Masalalar (1-10)

Masala 1 ⭐⭐

2-DOF robot qo'li: $L_1 = 0.5$ m, $L_2 = 0.3$ m. $\theta_1 = 30°$, $\theta_2 = 45°$ da end effector pozitsiyasi?

Yechim

import numpy as np

L1, L2 = 0.5, 0.3
theta1 = np.radians(30)
theta2 = np.radians(45)

x = L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2)
y = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)

print(f"x = {x:.3f} m")  # 0.545 m
print(f"y = {y:.3f} m")  # 0.499 m

---

Masala 2 ⭐⭐

Yuqoridagi robot uchun maqsad: $(0.6, 0.4)$ m. Burchaklarni toping.

Yechim

import numpy as np

L1, L2 = 0.5, 0.3
x, y = 0.6, 0.4

# theta2
cos_theta2 = (x**2 + y**2 - L1**2 - L2**2) / (2 * L1 * L2)
theta2 = np.arccos(cos_theta2)  # Elbow up

# theta1
theta1 = np.arctan2(y, x) - np.arctan2(L2*np.sin(theta2), L1 + L2*np.cos(theta2))

print(f"θ₁ = {np.degrees(theta1):.1f}°")  # 22.6°
print(f"θ₂ = {np.degrees(theta2):.1f}°")  # 63.9°

---

Masala 3 ⭐⭐

Differential drive: $v_R = 0.5$ m/s, $v_L = 0.3$ m/s, $L = 0.4$ m. Linear va angular tezlik?

Yechim

$v = \frac{0.5 + 0.3}{2} = 0.4$ m/s

$\omega = \frac{0.5 - 0.3}{0.4} = 0.5$ rad/s

---

Masala 4 ⭐⭐

Robot $v = 0.6$ m/s, $\omega = 0.2$ rad/s bilan harakatlanishi kerak. $L = 0.5$ m. G'ildirak tezliklari?

Yechim

$v_R = v + \frac{\omega L}{2} = 0.6 + \frac{0.2 \times 0.5}{2} = 0.65$ m/s

$v_L = v - \frac{\omega L}{2} = 0.6 - 0.05 = 0.55$ m/s

---

Masala 5 ⭐⭐

Encoder: 360 pulse/rev, g'ildirak radius 0.05 m. 1000 pulse = necha metr?

Yechim

Aylanish: $\frac{1000}{360} = 2.78$ rev

Masofa: $2.78 \times 2\pi \times 0.05 = 0.873$ m

---

Masala 6 ⭐⭐

2-DOF robot workspace: $L_1 = 0.4$ m, $L_2 = 0.3$ m. Min va max radius?

Yechim

$r_{min} = |L_1 - L_2| = |0.4 - 0.3| = 0.1$ m

$r_{max} = L_1 + L_2 = 0.4 + 0.3 = 0.7$ m

---

Masala 7 ⭐⭐

Odometriya: $\Delta s_R = 0.1$ m, $\Delta s_L = 0.08$ m, $L = 0.3$ m. $\Delta s$ va $\Delta\theta$?

Yechim

$\Delta s = \frac{0.1 + 0.08}{2} = 0.09$ m

$\Delta\theta = \frac{0.1 - 0.08}{0.3} = 0.067$ rad = 3.8°

---

Masala 8 ⭐⭐

Robot hozir $(1, 2)$ m, $\theta = 45°$. $\Delta s = 0.5$ m, $\Delta\theta = 10°$. Yangi pozitsiya?

Yechim

import numpy as np

x, y = 1, 2
theta = np.radians(45)
ds = 0.5
dtheta = np.radians(10)

x_new = x + ds * np.cos(theta + dtheta/2)
y_new = y + ds * np.sin(theta + dtheta/2)
theta_new = theta + dtheta

print(f"x = {x_new:.3f} m")  # 1.331 m
print(f"y = {y_new:.3f} m")  # 2.383 m
print(f"θ = {np.degrees(theta_new):.1f}°")  # 55°

---

Masala 9 ⭐⭐

SCARA robot: $L_1 = 0.3$ m, $L_2 = 0.2$ m. $(0.4, 0.2)$ ga yetish mumkinmi?

Yechim

$r = \sqrt{0.4^2 + 0.2^2} = 0.447$ m

$r_{min} = 0.1$ m, $r_{max} = 0.5$ m

$0.1 < 0.447 < 0.5$ → Ha, yetish mumkin

---

Masala 10 ⭐⭐

G'ildirak RPM = 60. Radius = 0.1 m. Linear tezlik?

Yechim

$\omega = 60 \times \frac{2\pi}{60} = 2\pi$ rad/s

$v = \omega r = 2\pi \times 0.1 = 0.628$ m/s

---

O'rtacha Masalalar (11-18)

Masala 11 ⭐⭐⭐

3-DOF robot qo'li: $L_1 = 0.4$, $L_2 = 0.3$, $L_3 = 0.2$ m. Forward kinematika.

Yechim

import numpy as np

def forward_3dof(theta1, theta2, theta3, L1=0.4, L2=0.3, L3=0.2):
    t1, t2, t3 = np.radians([theta1, theta2, theta3])
    
    x = L1*np.cos(t1) + L2*np.cos(t1+t2) + L3*np.cos(t1+t2+t3)
    y = L1*np.sin(t1) + L2*np.sin(t1+t2) + L3*np.sin(t1+t2+t3)
    phi = t1 + t2 + t3  # End effector orientation
    
    return x, y, phi

x, y, phi = forward_3dof(30, 45, -30)
print(f"Position: ({x:.3f}, {y:.3f}) m")
print(f"Orientation: {np.degrees(phi):.1f}°")

---

Masala 12 ⭐⭐⭐

2-DOF Jacobian: $L_1 = 0.5$, $L_2 = 0.3$ m, $\theta_1 = 45°$, $\theta_2 = 30°$. Jacobian matritsani hisoblang.

Yechim

import numpy as np

L1, L2 = 0.5, 0.3
t1, t2 = np.radians(45), np.radians(30)

J = np.array([
    [-L1*np.sin(t1) - L2*np.sin(t1+t2), -L2*np.sin(t1+t2)],
    [L1*np.cos(t1) + L2*np.cos(t1+t2), L2*np.cos(t1+t2)]
])

print("Jacobian:")
print(J)

# Determinant (singularity check)
det_J = np.linalg.det(J)
print(f"\nDeterminant: {det_J:.4f}")

---

Masala 13 ⭐⭐⭐

Singularlik: 2-DOF robot qachon singularlikda bo'ladi?

Yechim

$\det(J) = L_1 L_2 \sin\theta_2 = 0$

Singularlik sharti: $\theta_2 = 0°$ yoki $\theta_2 = 180°$

Bu holatda qo'l to'liq cho'zilgan yoki to'liq bukilgan.

# Singularity demonstration
theta2_values = [0, 30, 60, 90, 120, 150, 180]

for t2 in theta2_values:
    det = 0.5 * 0.3 * np.sin(np.radians(t2))
    status = "SINGULAR!" if abs(det) < 0.001 else "OK"
    print(f"θ₂ = {t2:3d}°: det(J) = {det:.4f} {status}")

---

Masala 14 ⭐⭐⭐

Cubic trajectory: $\theta_0 = 0°$, $\theta_f = 90°$, $T = 2$ s. $\theta(t)$ ni toping.

Yechim

Chegaralar: $\theta(0) = 0$, $\theta(2) = 90$, $\dot{\theta}(0) = 0$, $\dot{\theta}(2) = 0$

$$\theta(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3$$

Yechim: $a_0 = 0$, $a_1 = 0$, $a_2 = \frac{3 \times 90}{4} = 67.5$, $a_3 = -\frac{2 \times 90}{8} = -22.5$

$$\theta(t) = 67.5t^2 - 22.5t^3 \text{ (degrees)}$$

import numpy as np
import matplotlib.pyplot as plt

T = 2
theta0, thetaf = 0, 90

a0 = theta0
a1 = 0
a2 = 3*(thetaf - theta0) / T**2
a3 = -2*(thetaf - theta0) / T**3

t = np.linspace(0, T, 100)
theta = a0 + a1*t + a2*t**2 + a3*t**3
omega = a1 + 2*a2*t + 3*a3*t**2

plt.figure(figsize=(10, 4))
plt.subplot(1, 2, 1)
plt.plot(t, theta)
plt.title('Position')

plt.subplot(1, 2, 2)
plt.plot(t, omega)
plt.title('Velocity')

plt.show()

---

Masala 15 ⭐⭐⭐

DH parametrlari: 2-DOF planar robot.

Yechim

Link	$a_i$	$\alpha_i$	$d_i$	$\theta_i$
1	$L_1$	0	0	$\theta_1$
2	$L_2$	0	0	$\theta_2$

import numpy as np

def dh_matrix(a, alpha, d, theta):
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), np.sin(alpha)
    
    return np.array([
        [ct, -st*ca, st*sa, a*ct],
        [st, ct*ca, -ct*sa, a*st],
        [0, sa, ca, d],
        [0, 0, 0, 1]
    ])

L1, L2 = 0.5, 0.3
t1, t2 = np.radians(30), np.radians(45)

T01 = dh_matrix(L1, 0, 0, t1)
T12 = dh_matrix(L2, 0, 0, t2)
T02 = T01 @ T12

print("End effector position:")
print(f"x = {T02[0,3]:.3f} m")
print(f"y = {T02[1,3]:.3f} m")

---

Masala 16 ⭐⭐⭐

Aylana bo'ylab harakat: radius 0.2 m, markaz (0.4, 0.3), T = 10 s.

Yechim

import numpy as np

def circle_trajectory(t, cx=0.4, cy=0.3, r=0.2, T=10):
    omega = 2 * np.pi / T
    x = cx + r * np.cos(omega * t)
    y = cy + r * np.sin(omega * t)
    return x, y

# Inverse kinematics kerak har bir nuqta uchun
def inverse_2dof(x, y, L1=0.5, L2=0.3):
    cos_t2 = (x**2 + y**2 - L1**2 - L2**2) / (2*L1*L2)
    if abs(cos_t2) > 1:
        return None, None  # Out of reach
    
    t2 = np.arccos(cos_t2)
    t1 = np.arctan2(y, x) - np.arctan2(L2*np.sin(t2), L1+L2*np.cos(t2))
    return t1, t2

# Trajectory
t_vals = np.linspace(0, 10, 100)
for t in t_vals[::20]:
    x, y = circle_trajectory(t)
    t1, t2 = inverse_2dof(x, y)
    if t1 is not None:
        print(f"t={t:.1f}s: ({x:.2f}, {y:.2f}) -> θ1={np.degrees(t1):.1f}°, θ2={np.degrees(t2):.1f}°")

---

Masala 17 ⭐⭐⭐

Mecanum wheel robot: $v_x$, $v_y$, $\omega$ berilgan. 4 ta g'ildirak tezligi.

Yechim

import numpy as np

def mecanum_inverse(vx, vy, omega, L=0.2, W=0.15, r=0.05):
    """
    L: wheelbase/2
    W: track width/2
    r: wheel radius
    """
    # Wheel angular velocities
    w1 = (vx - vy - (L+W)*omega) / r  # Front Left
    w2 = (vx + vy + (L+W)*omega) / r  # Front Right
    w3 = (vx + vy - (L+W)*omega) / r  # Rear Left
    w4 = (vx - vy + (L+W)*omega) / r  # Rear Right
    
    return w1, w2, w3, w4

# Example: Move diagonally while rotating
vx, vy, omega = 0.5, 0.3, 0.2

w1, w2, w3, w4 = mecanum_inverse(vx, vy, omega)
print(f"FL: {w1:.1f} rad/s")
print(f"FR: {w2:.1f} rad/s")
print(f"RL: {w3:.1f} rad/s")
print(f"RR: {w4:.1f} rad/s")

---

Masala 18 ⭐⭐⭐

Odometriya drift: 10 m yo'l bosib o'tgandan keyin 5% xato. Xato qancha?

Yechim

Pozitsiya xatosi: $10 \times 0.05 = 0.5$ m

Drift sabablari:

• G'ildirak sirpanishi
• Encoder resolution
• Mexanik toleranslar

Kamaytirish usullari:

• IMU fusion (Kalman filter)
• Visual odometry
• GPS/SLAM tuzatish

---

Murakkab Masalalar (19-25)

Masala 19 ⭐⭐⭐⭐

Velocity control: End effector $v_x = 0.1$ m/s bilan harakatlanishi kerak. Bo'g'in tezliklari?

Yechim

import numpy as np

def compute_joint_velocities(vx, vy, theta1, theta2, L1=0.5, L2=0.3):
    # Jacobian
    t1, t2 = theta1, theta2
    J = np.array([
        [-L1*np.sin(t1) - L2*np.sin(t1+t2), -L2*np.sin(t1+t2)],
        [L1*np.cos(t1) + L2*np.cos(t1+t2), L2*np.cos(t1+t2)]
    ])
    
    # Cartesian velocity
    v_cart = np.array([vx, vy])
    
    # Joint velocities: q_dot = J^(-1) * x_dot
    J_inv = np.linalg.inv(J)
    q_dot = J_inv @ v_cart
    
    return q_dot

# Example
t1, t2 = np.radians(45), np.radians(30)
q_dot = compute_joint_velocities(0.1, 0, t1, t2)

print(f"θ₁_dot = {np.degrees(q_dot[0]):.2f} °/s")
print(f"θ₂_dot = {np.degrees(q_dot[1]):.2f} °/s")

---

Masala 20 ⭐⭐⭐⭐

SCARA robot DH parametrlari va forward kinematika.

Yechim

import numpy as np

# SCARA: 2 revolute (horizontal) + 1 prismatic + 1 revolute
# DH parameters:
# | Link | a    | alpha | d     | theta  |
# |------|------|-------|-------|--------|
# | 1    | L1   | 0     | d1    | theta1 |
# | 2    | L2   | π     | 0     | theta2 |
# | 3    | 0    | 0     | d3    | 0      |
# | 4    | 0    | 0     | 0     | theta4 |

def scara_fk(theta1, theta2, d3, theta4, L1=0.3, L2=0.25, d1=0.2):
    t1, t2, t4 = np.radians([theta1, theta2, theta4])
    
    x = L1*np.cos(t1) + L2*np.cos(t1+t2)
    y = L1*np.sin(t1) + L2*np.sin(t1+t2)
    z = d1 - d3
    phi = t1 + t2 + t4
    
    return x, y, z, phi

# Test
x, y, z, phi = scara_fk(30, 45, 0.1, 0)
print(f"Position: ({x:.3f}, {y:.3f}, {z:.3f}) m")
print(f"Orientation: {np.degrees(phi):.1f}°")

---

Masala 21 ⭐⭐⭐⭐

Path interpolation: Point A dan B ga 100 nuqta orqali harakat.

Yechim

import numpy as np

def linear_path(start, end, n_points=100):
    """Chiziqli interpolyatsiya"""
    t = np.linspace(0, 1, n_points)
    path = []
    for ti in t:
        point = start + ti * (end - start)
        path.append(point)
    return np.array(path)

def inverse_2dof(x, y, L1=0.5, L2=0.3):
    cos_t2 = (x**2 + y**2 - L1**2 - L2**2) / (2*L1*L2)
    cos_t2 = np.clip(cos_t2, -1, 1)
    t2 = np.arccos(cos_t2)
    t1 = np.arctan2(y, x) - np.arctan2(L2*np.sin(t2), L1+L2*np.cos(t2))
    return t1, t2

# Path planning
start = np.array([0.4, 0.3])
end = np.array([0.6, 0.5])

path = linear_path(start, end, 100)

# Convert to joint space
joint_path = []
for point in path:
    t1, t2 = inverse_2dof(point[0], point[1])
    joint_path.append([np.degrees(t1), np.degrees(t2)])

joint_path = np.array(joint_path)
print("Start joints:", joint_path[0])
print("End joints:", joint_path[-1])

---

Masala 22 ⭐⭐⭐⭐

Workspace visualization: 2-DOF robot uchun reachable workspace chizish.

Yechim

import numpy as np
import matplotlib.pyplot as plt

def forward_2dof(t1, t2, L1=0.5, L2=0.3):
    x = L1*np.cos(t1) + L2*np.cos(t1+t2)
    y = L1*np.sin(t1) + L2*np.sin(t1+t2)
    return x, y

# Generate workspace
theta1_range = np.linspace(-np.pi, np.pi, 100)
theta2_range = np.linspace(-np.pi, np.pi, 100)

points_x = []
points_y = []

for t1 in theta1_range:
    for t2 in theta2_range:
        x, y = forward_2dof(t1, t2)
        points_x.append(x)
        points_y.append(y)

plt.figure(figsize=(8, 8))
plt.scatter(points_x, points_y, s=1, alpha=0.5)
plt.xlabel('X (m)')
plt.ylabel('Y (m)')
plt.title('2-DOF Robot Workspace')
plt.axis('equal')
plt.grid(True)
plt.savefig('workspace.png', dpi=150)
plt.show()

---

Masala 23 ⭐⭐⭐⭐

Torque calculation: Gravitatsiya kompensatsiyasi.

Yechim

import numpy as np

def gravity_torques(theta1, theta2, L1=0.5, L2=0.3, m1=2, m2=1, g=9.81):
    """
    Statik gravitatsiya momentlari
    """
    t1, t2 = theta1, theta2
    
    # Link 1 COM: L1/2
    # Link 2 COM: L1 + L2/2
    
    tau1 = (m1 * g * L1/2 * np.cos(t1) + 
            m2 * g * (L1*np.cos(t1) + L2/2*np.cos(t1+t2)))
    
    tau2 = m2 * g * L2/2 * np.cos(t1+t2)
    
    return tau1, tau2

# Horizontal position
t1, t2 = 0, 0
tau1, tau2 = gravity_torques(t1, t2)
print(f"τ₁ = {tau1:.2f} Nm")
print(f"τ₂ = {tau2:.2f} Nm")

# Vertical position
t1, t2 = np.pi/2, 0
tau1, tau2 = gravity_torques(t1, t2)
print(f"\nVertical:")
print(f"τ₁ = {tau1:.2f} Nm")
print(f"τ₂ = {tau2:.2f} Nm")

---

Masala 24 ⭐⭐⭐⭐

Pure pursuit: Robot to'g'ri chiziqqa yaqinlashish.

Yechim

import numpy as np

def pure_pursuit(robot_pos, robot_heading, path, lookahead=0.5):
    """
    Pure pursuit controller
    """
    x, y = robot_pos
    theta = robot_heading
    
    # Find closest point on path
    min_dist = float('inf')
    closest_idx = 0
    
    for i, point in enumerate(path):
        dist = np.sqrt((point[0]-x)**2 + (point[1]-y)**2)
        if dist < min_dist:
            min_dist = dist
            closest_idx = i
    
    # Find lookahead point
    for i in range(closest_idx, len(path)):
        dist = np.sqrt((path[i][0]-x)**2 + (path[i][1]-y)**2)
        if dist >= lookahead:
            lookahead_point = path[i]
            break
    else:
        lookahead_point = path[-1]
    
    # Calculate steering
    dx = lookahead_point[0] - x
    dy = lookahead_point[1] - y
    alpha = np.arctan2(dy, dx) - theta
    
    # Curvature
    curvature = 2 * np.sin(alpha) / lookahead
    
    return curvature, lookahead_point

# Example
path = [(i*0.1, np.sin(i*0.1)) for i in range(100)]
robot = (0.5, 0.2)
heading = 0

curvature, target = pure_pursuit(robot, heading, path)
print(f"Curvature: {curvature:.3f}")
print(f"Target: {target}")

---

Masala 25 ⭐⭐⭐⭐

6-DOF robot: End effector pozitsiya va orientatsiyani Euler burchaklarida ifodalash.

Yechim

import numpy as np

def rotation_matrix_to_euler(R):
    """
    ZYX Euler burchaklari (yaw-pitch-roll)
    """
    sy = np.sqrt(R[0,0]**2 + R[1,0]**2)
    
    if sy > 1e-6:
        roll = np.arctan2(R[2,1], R[2,2])
        pitch = np.arctan2(-R[2,0], sy)
        yaw = np.arctan2(R[1,0], R[0,0])
    else:
        roll = np.arctan2(-R[1,2], R[1,1])
        pitch = np.arctan2(-R[2,0], sy)
        yaw = 0
    
    return np.degrees([roll, pitch, yaw])

# Example transformation matrix (from FK)
T = np.array([
    [0.866, -0.5, 0, 0.3],
    [0.5, 0.866, 0, 0.4],
    [0, 0, 1, 0.5],
    [0, 0, 0, 1]
])

R = T[:3, :3]
position = T[:3, 3]
euler = rotation_matrix_to_euler(R)

print(f"Position: {position}")
print(f"Orientation (RPY): {euler}")

---

✅ Tekshirish Ro'yxati

• 1-10: Forward/Inverse kinematika asoslari
• 11-18: Jacobian, DH, trajectory
• 19-25: Murakkab robotika

---

Keyingi Qadam

🔬 Amaliyot — Robot simulyatsiyasi!