Add files via upload

ch02_configuration_space/Explicit_Representation.py

@@ -0,0 +1,29 @@
+# 명시적 표현 (Explicit Representation)
+# 자유도만큼의 파라미터로 직접 공간을 표현
+# 예: S1을 각도 θ ∈ [0, 2π)로 표현
+
+import sys
+from pathlib import Path
+sys.path.append(str(Path(__file__).parent.parent))
+
+import numpy as np
+import matplotlib.pyplot as plt
+from ch02_configuration_space.topology import S1
+
+def explicit_representation_S1():
+    """S1(원)을 각도 파라미터로 명시적 표현"""
+    theta = np.linspace(0, 2*np.pi, 100)
+    radius = 1.0
+
+    x, y = S1(theta, radius)
+
+    plt.figure(figsize=(6, 6))
+    plt.plot(x, y, 'b-', linewidth=2)
+    plt.axis('equal')
+    plt.grid(True)
+    plt.title('Explicit Representation of S¹: (cos θ, sin θ)')
+    plt.xlabel('x')
+    plt.ylabel('y')
+    plt.show()
+
+    return x, y

ch02_configuration_space/Implicit_Representation.py

@@ -0,0 +1,28 @@
+# 암묵적 표현 (Implicit Representation)
+# 공간의 자유도보다 높은 차원 공간에서 제약조건을 거는 방식
+# 1차원 원 S1을 2차원 평면 R2에서 x^2 + y^2 - 1 = 0 으로 표현
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+def implicit_representation_S1():
+    """S1(원)을 2D 평면에서 제약조건으로 암묵적 표현"""
+    # 2D 그리드 생성
+    x = np.linspace(-1.5, 1.5, 400)
+    y = np.linspace(-1.5, 1.5, 400)
+    X, Y = np.meshgrid(x, y)
+
+    # 제약조건: g(x, y) = x^2 + y^2 - 1 = 0
+    Z = X**2 + Y**2 - 1
+
+    plt.figure(figsize=(6, 6))
+    # 등고선에서 0인 부분만 그리기 (제약조건을 만족하는 점들)
+    plt.contour(X, Y, Z, levels=[0], colors='red', linewidths=2)
+    plt.axis('equal')
+    plt.grid(True)
+    plt.title('Implicit Representation of S¹: x² + y² - 1 = 0')
+    plt.xlabel('x')
+    plt.ylabel('y')
+    plt.show()
+
+    return X, Y, Z

ch02_configuration_space/__init__.py

@@ -0,0 +1,22 @@
+"""
+Chapter 2: Configuration Space
+로봇의 형상 공간(C-space) 관련 모듈
+"""
+
+from .topology import S1, visualize_T2_from_S1
+from .Explicit_Representation import explicit_representation_S1
+from .Implicit_Representation import implicit_representation_S1
+from .constraints import g_holonomic, A_pfaffian, check_pfaffian_constraint
+from .c_space import gruebler_formula, visualize_2link_cspace
+
+__all__ = [
+    'S1',
+    'visualize_T2_from_S1',
+    'explicit_representation_S1',
+    'implicit_representation_S1',
+    'g_holonomic',
+    'A_pfaffian',
+    'check_pfaffian_constraint',
+    'gruebler_formula',
+    'visualize_2link_cspace',
+]

ch02_configuration_space/c_space.py

@@ -0,0 +1,59 @@
+import sys
+from pathlib import Path
+sys.path.append(str(Path(__file__).parent.parent))
+
+import numpy as np
+import matplotlib.pyplot as plt
+from ch02_configuration_space.topology import S1, visualize_T2_from_S1
+from ch02_configuration_space.Explicit_Representation import explicit_representation_S1
+from ch02_configuration_space.Implicit_Representation import implicit_representation_S1
+from ch02_configuration_space.constraints import g_holonomic, check_pfaffian_constraint
+
+def gruebler_formula(m, N, J, f_i):
+    """
+    Grübler 공식: 로봇의 자유도(DOF) 계산
+
+    Parameters:
+    - m: 공간의 차원 (평면: 3, 공간: 6)
+    - N: 링크(body) 개수 (고정된 ground 제외)
+    - J: 조인트 개수
+    - f_i: 각 조인트의 자유도 합
+
+    Returns:
+    - dof: 시스템의 자유도
+
+    공식: dof = m(N - 1 - J) + f_i
+    """
+    dof = m * (N - 1 - J) + f_i
+    return dof
+
+
+def visualize_2link_cspace():
+    """2-링크 로봇의 C-space (T²) 시각화"""
+    print("2-링크 로봇의 Configuration Space는 T² (토러스)입니다.")
+    print("각 조인트가 S¹이고, S¹ × S¹ = T²")
+    visualize_T2_from_S1()
+
+
+if __name__ == "__main__":
+    # 1. Grübler 공식 테스트
+    print("=== Grübler 공식 예제 ===")
+    print("평면 2-링크 로봇:")
+    # m=3 (평면), N=2 (링크 2개), J=2 (조인트 2개), f_i=2 (각 조인트 1 DOF씩)
+    dof = gruebler_formula(m=3, N=2, J=2, f_i=2)
+    print(f"자유도: {dof}")
+    print(f"계산: dof = m(N-1-J) + f_i = 3(2-1-2) + 2 = 3(-1) + 2 = -1")
+    print("이건 잘못된 결과입니다. N은 ground 포함해야 합니다!\n")
+
+    print("올바른 계산 (N=3, ground 포함):")
+    dof_correct = gruebler_formula(m=3, N=3, J=2, f_i=2)
+    print(f"자유도: {dof_correct}")
+    print(f"계산: dof = 3(3-1-2) + 2 = 3(0) + 2 = 2 ✓\n")
+
+    # 2. 명시적/암묵적 표현 비교
+    print("=== S¹의 두 가지 표현 ===")
+    explicit_representation_S1()
+    implicit_representation_S1()
+
+    # 3. 2-링크 로봇 C-space
+    visualize_2link_cspace()

ch02_configuration_space/constraints.py

@@ -0,0 +1,51 @@
+import numpy as np
+
+# ==========================================
+# 1. 홀로노믹 제약조건 (Holonomic Constraint)
+# ==========================================
+def g_holonomic(theta):
+    """
+    예시: 2차원 평면의 점(x, y)이 반지름 R=5인 원 위에 있어야 함.
+    수식: g(x, y) = x^2 + y^2 - R^2 = 0
+    """
+    x, y = theta
+    R = 5.0
+    constraint_value = x**2 + y**2 - R**2
+    return constraint_value
+
+# 테스트 (3, 4) 위치는 3^2 + 4^2 = 25 이므로 제약조건 만족(0)
+theta_valid = np.array([3.0, 4.0])
+print(f"홀로노믹 제약조건 평가 (3, 4): {g_holonomic(theta_valid)} (0이면 만족)")
+
+
+# ==========================================
+# 2. 파피안 제약조건 (Pfaffian Constraint)
+# ==========================================
+def A_pfaffian(theta):
+    """
+    예시: 외발 자전거(Unicycle) 로봇 (논홀로노믹의 대표적 예)
+    상태변수: theta = [x, y, phi(헤딩 각도)]
+    제약조건: 자동차는 옆으로 미끄러질 수 없다. (측면 속도 = 0)
+    수식: x_dot * sin(phi) - y_dot * cos(phi) = 0
+    A(theta) 행렬 = [sin(phi), -cos(phi), 0]
+    """
+    x, y, phi = theta
+    return np.array([np.sin(phi), -np.cos(phi), 0.0])
+
+def check_pfaffian_constraint(theta, theta_dot):
+    """ A(theta) * theta_dot = 0 인지 확인 """
+    A = A_pfaffian(theta)
+    # 행렬과 벡터의 내적 (Dot product)
+    violation = np.dot(A, theta_dot)
+    return violation
+
+# 테스트: 로봇이 45도(pi/4) 방향을 보고 있을 때
+theta_robot = np.array([0.0, 0.0, np.pi/4])
+
+# 속도 1: 앞으로 전진하는 속도 (올바른 움직임) -> 옆으로 안 미끄러짐
+theta_dot_forward = np.array([np.cos(np.pi/4), np.sin(np.pi/4), 0.0])
+print(f"\n파피안 제약조건 평가 (전진): {check_pfaffian_constraint(theta_robot, theta_dot_forward):.2f} (0이면 만족)")
+
+# 속도 2: 옆으로 게걸음 치는 속도 (물리적으로 불가능한 움직임)
+theta_dot_sideways = np.array([-np.sin(np.pi/4), np.cos(np.pi/4), 0.0])
+print(f"파피안 제약조건 평가 (게걸음): {check_pfaffian_constraint(theta_robot, theta_dot_sideways):.2f} (0이 아니면 제약 위반!)")

ch02_configuration_space/topology.py

@@ -0,0 +1,121 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+
+# ==========================================
+# 기본 위상 공간 (Topological Spaces)
+# ==========================================
+
+def E(n):
+    """
+    유클리드 공간 E^n (Euclidean Space)
+    n차원 실수 공간 R^n
+    """
+    return f"E^{n}: {n}차원 유클리드 공간"
+
+
+def S1(angle, radius=1.0):
+    """
+    S^1: 1차원 원 (Circle)
+    매개변수: angle ∈ [0, 2π)
+    반환: (x, y) = (r*cos(θ), r*sin(θ))
+    """
+    return radius * np.cos(angle), radius * np.sin(angle)
+
+
+def T(n):
+    """
+    n차원 토러스 T^n = S^1 × S^1 × ... × S^1 (n번)
+    예: T^2 = S^1 × S^1 (도넛 모양)
+        T^3 = S^1 × S^1 × S^1 (3-링크 로봇의 C-space)
+    """
+    return f"T^{n}: {n}차원 토러스 (S^1을 {n}번 곱한 공간)"
+
+
+# ==========================================
+# 시각화 함수들
+# ==========================================
+
+def visualize_T2_from_S1():
+    """T^2 = S^1 × S^1 시각화 (2-링크 로봇)"""
+    fig = plt.figure(figsize=(8, 6))
+    ax = fig.add_subplot(111, projection='3d')
+
+    theta1 = np.linspace(0, 2*np.pi, 50)
+    theta2 = np.linspace(0, 2*np.pi, 50)
+    U, V = np.meshgrid(theta1, theta2)
+
+    R = 2.0  # Major radius
+    r = 1.0  # Minor radius
+
+    x_small, z_small = S1(V, r)
+    dir_x, dir_y = S1(U, 1.0)
+
+    X = (R + x_small) * dir_x
+    Y = (R + x_small) * dir_y
+    Z = z_small
+
+    ax.plot_surface(X, Y, Z, alpha=0.8, cmap='plasma', edgecolor='gray')
+    ax.set_title("T² = S¹ × S¹ (2-Link Robot C-Space)")
+    ax.set_xlabel("X")
+    ax.set_ylabel("Y")
+    ax.set_zlabel("Z")
+    ax.set_box_aspect([1, 1, r/R])
+    plt.show()
+
+
+def visualize_T3_projection():
+    """
+    T^3 = S^1 × S^1 × S^1 시각화 (3-링크 로봇)
+    4차원 공간이므로 3차원으로 투영해서 표현
+    각 축이 하나의 조인트 각도를 나타냄
+    """
+    fig = plt.figure(figsize=(10, 8))
+    ax = fig.add_subplot(111, projection='3d')
+
+    # 3개의 조인트 각도
+    n_samples = 20
+    theta1 = np.linspace(0, 2*np.pi, n_samples)
+    theta2 = np.linspace(0, 2*np.pi, n_samples)
+    theta3 = np.linspace(0, 2*np.pi, n_samples)
+
+    # 샘플링된 점들을 3D 공간에 표시
+    # 실제로는 3차원 토러스를 완전히 시각화할 수 없지만,
+    # 각 조인트의 주기성을 보여줄 수 있음
+    for t1 in theta1[::4]:
+        for t2 in theta2[::4]:
+            x, y = S1(theta3, 1.0)
+            z = np.ones_like(x) * t1
+            ax.plot(x + t2/np.pi, y, z, 'b-', alpha=0.3, linewidth=0.5)
+
+    ax.set_title("T³ = S¹ × S¹ × S¹ (3-Link Robot C-Space)\n투영된 표현")
+    ax.set_xlabel("θ₁, θ₂ 방향")
+    ax.set_ylabel("θ₃ 방향")
+    ax.set_zlabel("높이")
+    plt.show()
+
+
+# ==========================================
+# 사용 예제
+# ==========================================
+
+if __name__ == "__main__":
+    print("=== 기본 위상 공간 ===")
+    print(E(2))  # 평면
+    print(E(3))  # 3차원 공간
+    print(T(2))  # 2-링크 로봇
+    print(T(3))  # 3-링크 로봇
+    print()
+
+    print("=== S^1 예제 ===")
+    angles = np.array([0, np.pi/2, np.pi])
+    for angle in angles:
+        x, y = S1(angle)
+        print(f"θ={angle:.2f}: ({x:.2f}, {y:.2f})")
+    print()
+
+    print("=== T^2 시각화 ===")
+    visualize_T2_from_S1()
+
+    print("=== T^3 시각화 ===")
+    visualize_T3_projection()