From 1d257d5d4f7e9b47e7d66f057cb550f50cd9ba97 Mon Sep 17 00:00:00 2001 From: CheolMin Yoon Date: Thu, 12 Mar 2026 08:15:43 +0900 Subject: [PATCH] Add files via upload --- .../Explicit_Representation.py | 29 +++++ .../Implicit_Representation.py | 28 ++++ ch02_configuration_space/__init__.py | 22 ++++ ch02_configuration_space/c_space.py | 59 +++++++++ ch02_configuration_space/constraints.py | 51 ++++++++ ch02_configuration_space/topology.py | 121 ++++++++++++++++++ 6 files changed, 310 insertions(+) create mode 100644 ch02_configuration_space/Explicit_Representation.py create mode 100644 ch02_configuration_space/Implicit_Representation.py create mode 100644 ch02_configuration_space/__init__.py create mode 100644 ch02_configuration_space/c_space.py create mode 100644 ch02_configuration_space/constraints.py create mode 100644 ch02_configuration_space/topology.py diff --git a/ch02_configuration_space/Explicit_Representation.py b/ch02_configuration_space/Explicit_Representation.py new file mode 100644 index 0000000..ea07aac --- /dev/null +++ b/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 \ No newline at end of file diff --git a/ch02_configuration_space/Implicit_Representation.py b/ch02_configuration_space/Implicit_Representation.py new file mode 100644 index 0000000..b9112a8 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/ch02_configuration_space/__init__.py b/ch02_configuration_space/__init__.py new file mode 100644 index 0000000..3b61ddb --- /dev/null +++ b/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', +] diff --git a/ch02_configuration_space/c_space.py b/ch02_configuration_space/c_space.py new file mode 100644 index 0000000..474c18c --- /dev/null +++ b/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() \ No newline at end of file diff --git a/ch02_configuration_space/constraints.py b/ch02_configuration_space/constraints.py new file mode 100644 index 0000000..9f93bbf --- /dev/null +++ b/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이 아니면 제약 위반!)") \ No newline at end of file diff --git a/ch02_configuration_space/topology.py b/ch02_configuration_space/topology.py new file mode 100644 index 0000000..57a1ab4 --- /dev/null +++ b/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() \ No newline at end of file