12궤적 계획 — 부드럽게 움직이기

import math

dt = 1 / 100                       # 100 Hz 제어 주기
dq = math.radians(90)              # joint2: 0° → 90°

v_jump = dq / dt                   # 한 주기 만에 가려면 필요한 속도
print(f"한 주기에 점프: {v_jump:.0f} rad/s = {math.degrees(v_jump):.0f} °/s")
print(f"2초에 나눠 가기: {dq / 2:.3f} rad/s = {90 / 2:.0f} °/s")

한 주기에 점프: 157 rad/s = 9000 °/s
2초에 나눠 가기: 0.785 rad/s = 45 °/s

q(s) = q0 + (q1 − q0) · s        s = 0 이면 출발, s = 1 이면 도착

def lerp_pose(q0, q1, s):
    return [a + (b - a) * s for a, b in zip(q0, q1)]

q0 = [0, 45, 90, 0, 0, 0]
q1 = [40, 70, 110, -30, 20, 0]
for i in range(5):
    s = i / 4
    print(f"s = {s:.2f} →", [round(v, 1) for v in lerp_pose(q0, q1, s)])

s = 0.00 → [0.0, 45.0, 90.0, 0.0, 0.0, 0.0]
s = 0.25 → [10.0, 51.2, 95.0, -7.5, 5.0, 0.0]
s = 0.50 → [20.0, 57.5, 100.0, -15.0, 10.0, 0.0]
s = 0.75 → [30.0, 63.8, 105.0, -22.5, 15.0, 0.0]
s = 1.00 → [40.0, 70.0, 110.0, -30.0, 20.0, 0.0]

def linear(t):
    return t

def smoothstep(t):
    return 3 * t**2 - 2 * t**3

def quintic(t):
    return 10 * t**3 - 15 * t**4 + 6 * t**5

N = 10
for name, f in [("linear", linear), ("smoothstep", smoothstep), ("quintic", quintic)]:
    speeds = [(f((i + 1) / N) - f(i / N)) * N for i in range(N)]   # 구간별 속도
    bars = "".join(" ▁▂▃▄▅▆▇█"[min(8, round(v / 1.875 * 8))] for v in speeds)
    print(f"{name:>10} |{bars}|  최고 {max(speeds):.2f}")

    linear |▄▄▄▄▄▄▄▄▄▄|  최고 1.00
smoothstep |▁▃▅▆▆▆▆▅▃▁|  최고 1.48
   quintic | ▂▄▇██▇▄▂ |  최고 1.83

import math

def trapezoid_time(D, vmax, amax):
    """거리 D 를 vmax·amax 를 지키며 가는 최소 시간과 가속 시간"""
    ta = vmax / amax                        # 최고 속도까지 걸리는 시간
    if D < vmax * ta:                       # 너무 짧아 최고 속도에 못 닿음 → 삼각형
        ta = math.sqrt(D / amax)
        return 2 * ta, ta
    return D / vmax + ta, ta

def trapezoid_s(t, T, ta):
    """시간 t 에서의 진행률 s (0~1)"""
    vp = 1 / (T - ta)                       # 정규화된 최고 속도
    if t < ta:
        return 0.5 * vp / ta * t * t
    if t < T - ta:
        return 0.5 * vp * ta + vp * (t - ta)
    r = T - t
    return 1 - 0.5 * vp / ta * r * r

for D in [90, 10]:                          # 관절을 90°, 10° 움직이기
    T, ta = trapezoid_time(D, vmax=30, amax=60)
    shape = "사다리꼴" if T > 2 * ta + 1e-9 else "삼각형"
    print(f"{D}° 이동: 총 {T:.2f} s, 가속 {ta:.2f} s ({shape})")
    print("   s:", [round(trapezoid_s(T * i / 6, T, ta), 2) for i in range(7)])

90° 이동: 총 3.50 s, 가속 0.50 s (사다리꼴)
   s: [0.0, 0.11, 0.31, 0.5, 0.69, 0.89, 1.0]
10° 이동: 총 0.82 s, 가속 0.41 s (삼각형)
   s: [0.0, 0.06, 0.22, 0.5, 0.78, 0.94, 1.0]

def quintic(t):
    return 10 * t**3 - 15 * t**4 + 6 * t**5

def plan(q0, q1, T, dt=0.05, profile=quintic):
    """q0 → q1 을 T 초 동안, dt 간격 자세 리스트로"""
    n = max(1, round(T / dt))
    traj = []
    for i in range(n + 1):
        s = profile(i / n)
        traj.append([a + (b - a) * s for a, b in zip(q0, q1)])
    return traj

arm.home()
q0 = arm.get_joints()
q1 = arm.ik(0.40, 0.10, 0.30)            # 목표를 IK 로 구하기 (위치만)
if q1 is None:
    print("IK 실패 — 목표를 바꾸세요")
else:
    dt = 0.05
    traj = plan(q0, q1, T=2.0, dt=dt)
    print("궤적 점 개수:", len(traj))
    vmax = 0
    for a, b in zip(traj, traj[1:]):     # 이웃한 두 점 사이의 관절 속도
        v = max(abs(y - x) for x, y in zip(a, b)) / dt
        vmax = max(vmax, v)
    print(f"가장 빠른 관절 속도: {vmax:.1f} °/s")
    for q in traj[1:]:
        arm.move_joints(q, t=dt)         # 짧은 명령을 차례로 쌓기

T = k · max|Δq| / vmax        (선형 k = 1, smoothstep k = 1.5, 5차 k = 1.875)

K = {"linear": 1.0, "smoothstep": 1.5, "quintic": 1.875}

def auto_duration(q0, q1, vmax=30.0, profile="quintic"):
    """모든 관절이 vmax(°/s)를 넘지 않는 가장 짧은 시간(초)"""
    worst = max(abs(b - a) for a, b in zip(q0, q1))
    return max(0.3, K[profile] * worst / vmax)

q0 = [0, 45, 90, 0, 0, 0]
q1 = [60, 80, 120, -30, 20, 0]
for p in K:
    print(f"{p:>10}: {auto_duration(q0, q1, profile=p):.2f} s")

    linear: 2.00 s
smoothstep: 3.00 s
   quintic: 3.75 s

import math

A = (0.30, -0.10, 0.10)
B = (0.25, 0.15, 0.10)
qA = arm.ik(*A, down=True)
qB = arm.ik(*B, down=True)

def dist_to_line(p, a, b):
    """점 p 에서 선분 a-b 까지의 거리"""
    ab = [b[i] - a[i] for i in range(3)]
    ap = [p[i] - a[i] for i in range(3)]
    t = sum(ab[i] * ap[i] for i in range(3)) / sum(v * v for v in ab)
    t = max(0, min(1, t))
    return math.dist(p, [a[i] + ab[i] * t for i in range(3)])

if qA and qB:
    worst = 0
    for i in range(11):
        s = i / 10
        q = [a + (b - a) * s for a, b in zip(qA, qB)]   # 관절 공간 보간
        p = arm.fk(q)
        worst = max(worst, dist_to_line(p, A, B))
    print(f"관절 공간 이동: 손끝이 직선에서 최대 {worst * 1000:.1f} mm 벗어남")

    arm.move_joints(qA, t=2)
    arm.move_joints(qB, t=2)          # 관절 공간 (곡선)
    arm.move_joints(qA, t=2)
    arm.move_line(*B, t=2, steps=20)  # 직교 공간 (직선)