• ArUco 마커 좌표 및 물리적 파라미터 수정 가이드
• Robot-Arm-Modification

ArUco 마커 좌표 및 물리적 파라미터 수정 가이드

1. ArUco 마커 좌표 수정

1.1 현재 마커 좌표 분석

파일 위치: non_ros_src/astra_teleop/src/astra_teleop/process.py

현재 마커 좌표 (SCARA 로봇팔 기준):

obj_points_map = { # right part
    # top_left, top_right, bottom_right, bottom_left
    0:  [ (-15.00, -15.00,  48.28), (-15.00,  15.00,  48.28), ( 15.00,  15.00,  48.28), ( 15.00, -15.00,  48.28) ],
    
    1:  [ ( 23.54, -15.00,  44.75), ( 23.54,  15.00,  44.75), ( 44.75,  15.00,  23.54), ( 44.75, -15.00,  23.54) ],
    2:  [ (-15.00, -23.54,  44.75), ( 15.00, -23.54,  44.75), ( 15.00, -44.75,  23.54), (-15.00, -44.75,  23.54) ],
    # ... 더 많은 마커들
}

1.2 새로운 로봇팔용 마커 좌표 측정 방법

1.2.1 측정 도구 준비

# 측정용 스크립트 예시
import numpy as np
import cv2
from astra_teleop.process import get_solve

def measure_marker_positions():
    """
    새로운 로봇팔에서 ArUco 마커들의 3D 좌표를 측정하는 함수
    """
    # 1. 카메라 캘리브레이션 (이미 완료되어 있다고 가정)
    camera_matrix = np.array([fx, 0, cx], [0, fy, cy], [0, 0, 1](/dingdongdengdong/astra_ws/wiki/fx,-0,-cx],-[0,-fy,-cy],-[0,-0,-1))
    distortion_coefficients = np.array([k1, k2, p1, p2, k3])
    
    # 2. 각 마커의 실제 3D 위치 측정
    marker_positions = {}
    
    for marker_id in range(36):  # 0-35번 마커
        # 실제 로봇팔에서 마커 위치를 측정
        # 예: 레이저 측정기, 3D 스캐너, 또는 수동 측정
        x, y, z = measure_marker_3d_position(marker_id)
        
        # 마커의 4개 코너 좌표 계산 (30mm x 30mm 마커 기준)
        marker_size = 30.0  # mm
        half_size = marker_size / 2
        
        corners_3d = [
            (x - half_size, y - half_size, z),  # top_left
            (x + half_size, y - half_size, z),  # top_right  
            (x + half_size, y + half_size, z),  # bottom_right
            (x - half_size, y + half_size, z),  # bottom_left
        ]
        
        marker_positions[marker_id] = corners_3d
    
    return marker_positions

1.2.2 실제 측정 예시

기존 SCARA 로봇팔 마커 배치:

마커 0: (-15.00, -15.00, 48.28) mm  # 오른쪽 팔 상단
마커 1: ( 23.54, -15.00, 44.75) mm  # 오른쪽 팔 측면
마커 2: (-15.00, -23.54, 44.75) mm  # 오른쪽 팔 후면
...
마커 18: (-15.00, -15.00, 48.28) mm # 왼쪽 팔 상단 (기존과 동일)

새로운 자유6축 로봇팔 마커 배치 (예시):

마커 0: (-25.00, -20.00, 60.00) mm  # 더 큰 로봇팔, 더 넓은 간격
마커 1: ( 35.00, -20.00, 55.00) mm  # 팔 길이가 길어져서 더 멀리
마커 2: (-25.00, -35.00, 55.00) mm  # 작업 공간이 넓어짐
...
마커 18: (-25.00, -20.00, 60.00) mm # 왼쪽 팔도 동일한 패턴

1.3 수정된 마커 좌표 적용

1.3.1 process.py 수정 예시

# non_ros_src/astra_teleop/src/astra_teleop/process.py

def get_solve(scale=1):
    # 새로운 로봇팔용 마커 좌표
    obj_points_map = { # right part (새로운 자유6축 로봇팔)
        # top_left, top_right, bottom_right, bottom_left
        0:  [ (-25.00, -20.00,  60.00), (-25.00,  20.00,  60.00), ( 25.00,  20.00,  60.00), ( 25.00, -20.00,  60.00) ],
        
        1:  [ ( 35.00, -20.00,  55.00), ( 35.00,  20.00,  55.00), ( 55.00,  20.00,  35.00), ( 55.00, -20.00,  35.00) ],
        2:  [ (-25.00, -35.00,  55.00), ( 25.00, -35.00,  55.00), ( 25.00, -55.00,  35.00), (-25.00, -55.00,  35.00) ],
        3:  [ (-35.00,  20.00,  55.00), (-35.00, -20.00,  55.00), (-55.00, -20.00,  35.00), (-55.00,  20.00,  35.00) ],
        4:  [ ( 25.00,  35.00,  55.00), (-25.00,  35.00,  55.00), (-25.00,  55.00,  35.00), ( 25.00,  55.00,  35.00) ],
        
        5:  [ ( 60.00, -20.00,  20.00), ( 60.00,  20.00,  20.00), ( 60.00,  20.00, -20.00), ( 60.00, -20.00, -20.00) ],
        6:  [ ( 35.00, -55.00,  20.00), ( 55.00, -35.00,  20.00), ( 55.00, -35.00, -20.00), ( 35.00, -55.00, -20.00) ],
        7:  [ (-25.00, -60.00,  20.00), ( 25.00, -60.00,  20.00), ( 25.00, -60.00, -20.00), (-25.00, -60.00, -20.00) ],
        8:  [ (-55.00, -35.00,  20.00), (-35.00, -55.00,  20.00), (-35.00, -55.00, -20.00), (-55.00, -35.00, -20.00) ],
        9:  [ (-60.00,  20.00,  20.00), (-60.00, -20.00,  20.00), (-60.00, -20.00, -20.00), (-60.00,  20.00, -20.00) ],
        10: [ (-35.00,  55.00,  20.00), (-55.00,  35.00,  20.00), (-55.00,  35.00, -20.00), (-35.00,  55.00, -20.00) ],
        11: [ ( 25.00,  60.00,  20.00), (-25.00,  60.00,  20.00), (-25.00,  60.00, -20.00), ( 25.00,  60.00, -20.00) ],
        12: [ ( 55.00,  35.00,  20.00), ( 35.00,  55.00,  20.00), ( 35.00,  55.00, -20.00), ( 55.00,  35.00, -20.00) ],
        
        13: [ ( 55.00, -20.00, -35.00), ( 55.00,  20.00, -35.00), ( 35.00,  20.00, -55.00), ( 35.00, -20.00, -55.00) ],
        14: [ (-25.00, -55.00, -35.00), ( 25.00, -55.00, -35.00), ( 25.00, -35.00, -55.00), (-25.00, -35.00, -55.00) ],
        15: [ (-55.00,  20.00, -35.00), (-55.00, -20.00, -35.00), (-35.00, -20.00, -55.00), (-35.00,  20.00, -55.00) ],
        16: [ ( 25.00,  55.00, -35.00), (-25.00,  55.00, -35.00), (-25.00,  35.00, -55.00), ( 25.00,  35.00, -55.00) ],
    }
    
    # 왼쪽 팔 마커들 (18-35번) - 기존과 동일한 패턴으로 복사
    for i in range(18 - 1): # left part
        obj_points_map[i + 18] = obj_points_map[i]
    
    # 나머지 solve 함수는 동일...

1.3.2 스케일 팩터 조정

# teleoprator.py에서 스케일 팩터 조정
class Teleopoperator:
    def __init__(self):        
        # ... 기존 코드 ...
        
        # 새로운 로봇팔에 맞는 스케일 팩터 조정
        # 기존: scale=1.0
        # 새로운 로봇팔이 더 크다면: scale=1.2
        # 새로운 로봇팔이 더 작다면: scale=0.8
        self.solve = get_solve(scale=1.2)  # 예시: 20% 더 큰 로봇팔

2. 물리적 파라미터 조정

2.1 그리퍼 관련 파라미터

2.1.1 그리퍼 최대 개방 거리

파일 위치: non_ros_src/astra_teleop_web/src/astra_teleop_web/teleoprator.py

현재 설정:

GRIPPER_MAX = 0.055  # 55mm (기존 SCARA 로봇팔)

새로운 로봇팔용 수정 예시:

# 새로운 로봇팔의 그리퍼 크기에 맞게 조정
GRIPPER_MAX = 0.080  # 80mm (더 큰 그리퍼)
# 또는
GRIPPER_MAX = 0.040  # 40mm (더 작은 그리퍼)

2.1.2 그리퍼 제어 로직 확인

def pedal_cb(self, pedal_real_values):
    # ... 기존 코드 ...
    
    if self.teleop_mode == "arm":
        # ... 기존 코드 ...
        
        gripper_pos = {}
        for side in ["left", "right"]:
            # GRIPPER_MAX가 변경되면 자동으로 반영됨
            gripper_pos[side] = (1 - values[f"{side}-gripper"]) * GRIPPER_MAX

2.2 리프트 관련 파라미터

2.2.1 초기 리프트 높이

현재 설정:

INITIAL_LIFT_DISTANCE = 0.8  # 800mm (기존 SCARA 로봇팔)

새로운 로봇팔용 수정 예시:

# 새로운 로봇팔의 작업 높이에 맞게 조정
INITIAL_LIFT_DISTANCE = 1.0   # 1000mm (더 큰 로봇팔)
# 또는
INITIAL_LIFT_DISTANCE = 0.6   # 600mm (더 작은 로봇팔)

2.2.2 리프트 범위 및 속도

현재 설정 (pedal_cb 함수 내부):

LIFT_VEL_MAX = 0.5           # 리프트 속도
LIFT_DISTANCE_MIN = 0        # 최소 높이
LIFT_DISTANCE_MAX = 1.2      # 최대 높이

새로운 로봇팔용 수정 예시:

def pedal_cb(self, pedal_real_values):
    # ... 기존 코드 ...
    
    if self.teleop_mode == "arm":
        # 새로운 로봇팔에 맞는 리프트 파라미터
        LIFT_VEL_MAX = 0.3           # 더 안전한 속도
        LIFT_DISTANCE_MIN = 0.2      # 최소 높이 증가
        LIFT_DISTANCE_MAX = 1.5      # 최대 높이 증가
        
        # ... 나머지 로직은 동일

2.3 헤드 틸트 계산 함수

2.3.1 현재 설정

def get_head_tilt(self, lift_distance):
    point0_lift = 0
    point0_tilt = 1.36
    point1_lift = 0.8
    point1_tilt = 1.06
    
    return point0_tilt + (point1_tilt - point0_tilt) * (lift_distance - point0_lift) / (point1_lift - point0_lift)

2.3.2 새로운 로봇팔용 수정

def get_head_tilt(self, lift_distance):
    # 새로운 로봇팔의 작업 공간에 맞게 조정
    point0_lift = 0.2        # 최소 높이 증가
    point0_tilt = 1.36       # 최소 높이에서의 틸트 각도
    point1_lift = 1.0        # 최대 높이 증가 (INITIAL_LIFT_DISTANCE와 맞춤)
    point1_tilt = 1.06       # 최대 높이에서의 틸트 각도
    
    return point0_tilt + (point1_tilt - point0_tilt) * (lift_distance - point0_lift) / (point1_lift - point0_lift)

3. 실제 수정 작업 순서

3.1 1단계: 마커 좌표 측정

# 1. 새로운 로봇팔에서 마커 배치
# 2. 3D 좌표 측정 (레이저 측정기, 3D 스캐너 등)
# 3. 측정 결과를 Excel이나 CSV로 정리

3.2 2단계: 코드 수정

# 1. process.py의 obj_points_map 업데이트
# 2. teleoprator.py의 물리적 파라미터 조정
# 3. 스케일 팩터 조정

3.3 3단계: 테스트

# 마커 감지 테스트
from astra_teleop.process import get_solve

solve = get_solve(scale=1.2)  # 새로운 스케일
# 실제 카메라로 마커 감지 테스트

3.4 4단계: 통합 테스트

# 전체 텔레오퍼레이션 시스템 테스트
# - 마커 기반 손 추적 정확도 확인
# - 그리퍼 제어 범위 확인
# - 리프트 동작 범위 확인

4. 주의사항

4.1 좌표계 일관성

• 모든 마커 좌표가 동일한 기준점을 사용하는지 확인
• 왼쪽/오른쪽 팔의 대칭성 유지

4.2 단위 통일

• 모든 좌표는 mm 단위
• 모든 거리는 m 단위
• 모든 각도는 라디안 단위

4.3 안전성 확인

• 새로운 파라미터가 안전한 범위 내에 있는지 확인
• 그리퍼 개방 거리가 물리적 한계를 초과하지 않는지 확인
• 리프트 범위가 로봇팔의 기구학적 한계를 초과하지 않는지 확인

5. 예시: 전체 수정된 파일

5.1 수정된 process.py 예시

# non_ros_src/astra_teleop/src/astra_teleop/process.py

def get_solve(scale=1):
    # 새로운 자유6축 로봇팔용 마커 좌표
    obj_points_map = {
        # 오른쪽 팔 마커들 (0-17번)
        0:  [ (-25.00, -20.00,  60.00), (-25.00,  20.00,  60.00), ( 25.00,  20.00,  60.00), ( 25.00, -20.00,  60.00) ],
        1:  [ ( 35.00, -20.00,  55.00), ( 35.00,  20.00,  55.00), ( 55.00,  20.00,  35.00), ( 55.00, -20.00,  35.00) ],
        # ... 나머지 마커들
        
        # 왼쪽 팔 마커들 (18-35번) - 오른쪽과 동일한 패턴
        18: [ (-25.00, -20.00,  60.00), (-25.00,  20.00,  60.00), ( 25.00,  20.00,  60.00), ( 25.00, -20.00,  60.00) ],
        19: [ ( 35.00, -20.00,  55.00), ( 35.00,  20.00,  55.00), ( 55.00,  20.00,  35.00), ( 55.00, -20.00,  35.00) ],
        # ... 나머지 마커들
    }
    
    # 나머지 solve 함수는 동일...

5.2 수정된 teleoprator.py 예시

# non_ros_src/astra_teleop_web/src/astra_teleop_web/teleoprator.py

# 새로운 로봇팔에 맞는 물리적 파라미터
GRIPPER_MAX = 0.080              # 80mm (더 큰 그리퍼)
INITIAL_LIFT_DISTANCE = 1.0       # 1000mm (더 높은 작업 공간)
FAR_SEEING_HEAD_TILT = 0.26       # 동일

class Teleopoperator:
    def __init__(self):        
        # ... 기존 코드 ...
        
        # 새로운 로봇팔에 맞는 스케일 팩터
        self.solve = get_solve(scale=1.2)  # 20% 더 큰 로봇팔
        
        # ... 나머지 코드는 동일

Robot-Arm-Modification

개요 (Overview)

로봇팔의 크기와 형태가 변경될 때, 텔레오퍼레이션 시스템에서 수정해야 하는 부분들을 분석한 문서입니다. 이 문서는 astra_teleop_web 패키지의 구조를 기반으로 작성되었습니다.

원격조종 시스템: 6축 SCARA → 자유6축 로봇팔 전환

핵심 결론: 수정 필요 없음

원격조종 시스템의 관점에서 6축 SCARA에서 자유6축 로봇팔로 변경하는 것은 기존 텔레오퍼레이션 코드를 수정할 필요가 없습니다.

이유 분석

1. 추상화된 인터페이스 사용

현재 teleoprator.py는 로봇팔의 구체적인 기구학에 의존하지 않습니다:

# teleoprator.py의 핵심 콜백들
self.on_pub_goal = None          # 목표 위치 전송 (추상화됨)
self.on_pub_gripper = None       # 그리퍼 제어 (추상화됨)
self.on_get_current_eef_pose = None  # 현재 위치 조회 (추상화됨)
self.on_get_initial_eef_pose = None  # 초기 위치 조회 (추상화됨)

2. 좌표계 변환의 추상화

# hand_cb에서 로봇 명령 생성
Tsgoal = self.Tscam[side] @ self.Tcamgoal_last[side]
self.on_pub_goal(side, Tsgoal)  # 4x4 변환 행렬로 추상화됨

3. 기구학 독립적 설계

• 입력: ArUco 마커 → 3D 위치 (카메라 좌표계)
• 처리: 좌표계 변환 (카메라 → 로봇)
• 출력: 4x4 변환 행렬 (추상화된 로봇 명령)

실제 수정이 필요한 부분

1. 하위 레벨 제어기만 수정

# astra_controller 패키지에서만 수정 필요
# - arm_controller.py: 6축 자유도에 맞는 제어 로직
# - URDF 파일: 새로운 기구학 정의
# - MoveIt 설정: 새로운 기구학 반영

2. 콜백 함수들의 구현체만 변경

# teleoprator.py는 수정 불필요
# 대신 이 콜백들을 구현하는 하위 시스템만 수정:

def on_pub_goal_implementation(side, Tsgoal):
    # 6축 SCARA용 구현 → 6축 자유도용 구현으로 변경
    # 하지만 teleoprator.py는 이 구현체를 몰라도 됨
    
def on_get_current_eef_pose_implementation(side):
    # 6축 SCARA용 구현 → 6축 자유도용 구현으로 변경
    # 하지만 teleoprator.py는 이 구현체를 몰라도 됨

확인해야 할 부분

1. ArUco 마커 배치

# astra_teleop/process.py의 obj_points_map
# 로봇팔 크기가 변경되면 마커 좌표만 수정
obj_points_map = {
    0: [(-15.00, -15.00, 48.28), ...],  # 기존 좌표
    # → 새로운 로봇팔 크기에 맞는 좌표로 변경
}

2. 물리적 파라미터 (선택적)

# teleoprator.py의 상수들 (필요시만 조정)
GRIPPER_MAX = 0.055              # 그리퍼 크기가 다르면 조정
INITIAL_LIFT_DISTANCE = 0.8      # 작업 높이가 다르면 조정
LIFT_DISTANCE_MIN = 0            # 리프트 범위가 다르면 조정
LIFT_DISTANCE_MAX = 1.2          # 리프트 범위가 다르면 조정

결론

원격조종 시스템 자체는 수정할 필요가 없습니다.

• ✅ 유지되는 부분: teleoprator.py, webserver.py, ArUco 처리 로직
• 🔧 수정 필요한 부분: astra_controller 패키지의 하위 제어기들
• 📏 조정 가능한 부분: ArUco 마커 좌표, 물리적 파라미터 (선택적)

권장 작업 순서

1. 새로운 로봇팔의 ArUco 마커 좌표 측정 및 업데이트
2. astra_controller 패키지의 하위 제어기 수정
3. URDF 및 MoveIt 설정 업데이트
4. 필요시 물리적 파라미터 조정 (그리퍼 크기, 작업 높이 등)

이렇게 하면 기존 원격조종 시스템을 그대로 사용하면서 새로운 6축 자유도 로봇팔을 제어할 수 있습니다.

SCARA → 협동로봇 전환 특별 고려사항

1. 기구학적 차이점

1.1 SCARA vs 협동로봇의 주요 차이

• SCARA: 평면 운동, 고정된 Z축, 빠른 속도, 정밀한 반복 작업
• 협동로봇: 6축 자유도, 유연한 작업 공간, 안전 기능, 사람과의 협업

1.2 기구학적 수정 필요사항

현재 SCARA 기반 설정:

# teleoprator.py의 reset_arm() 함수
goal_pose = {
    "left": self.on_get_initial_eef_pose("left", [self.lift_distance, joint_bent, -joint_bent, 0, 0, 0]),
    "right": self.on_get_initial_eef_pose("right", [self.lift_distance, -joint_bent, joint_bent, 0, 0, 0]),
}

협동로봇용 수정:

# 협동로봇의 6축 자유도에 맞는 초기 자세 설정
def get_collaborative_robot_initial_pose(self, side):
    if side == "left":
        return [self.lift_distance, -math.pi/6, -math.pi/3, 0, math.pi/2, 0]  # 협동로봇 특성에 맞는 자세
    else:  # right
        return [self.lift_distance, math.pi/6, math.pi/3, 0, -math.pi/2, 0]

2. 안전성 관련 수정

2.1 속도 제한 및 안전 파라미터

현재 SCARA 설정:

LIFT_VEL_MAX = 0.5  # SCARA용 빠른 속도
LINEAR_VEL_MAX = 1   # 베이스 이동 속도
ANGULAR_VEL_MAX = 1  # 회전 속도

협동로봇용 수정:

# 협동로봇의 안전 속도 제한
COLLABORATIVE_LIFT_VEL_MAX = 0.2  # 더 낮은 속도로 안전성 확보
COLLABORATIVE_LINEAR_VEL_MAX = 0.5
COLLABORATIVE_ANGULAR_VEL_MAX = 0.5

# 추가 안전 파라미터
MAX_FORCE_THRESHOLD = 50.0  # N, 힘 제한
MAX_TORQUE_THRESHOLD = 10.0  # Nm, 토크 제한
EMERGENCY_STOP_DISTANCE = 0.3  # m, 긴급 정지 거리

2.2 충돌 감지 및 회피

class CollaborativeRobotSafety:
    def __init__(self):
        self.force_sensor_active = True
        self.proximity_sensor_active = True
        self.emergency_stop_active = False
    
    def check_safety_limits(self, current_pose, target_pose):
        # 힘/토크 제한 확인
        if self.detect_excessive_force():
            return False, "Force limit exceeded"
        
        # 근접 감지 확인
        if self.detect_obstacle_proximity():
            return False, "Obstacle detected"
        
        return True, "Safe to move"
    
    def emergency_stop(self):
        self.emergency_stop_active = True
        # 모든 모터 정지
        self.stop_all_motors()

3. 제어 모드 수정

3.1 협동로봇 전용 제어 모드

# teleoprator.py에 추가할 협동로봇 모드
COLLABORATIVE_MODES = {
    "position_control": "정밀 위치 제어",
    "force_control": "힘 제어 모드", 
    "impedance_control": "임피던스 제어",
    "gravity_compensation": "중력 보상 모드",
    "manual_guidance": "수동 가이던스"
}

def update_collaborative_mode(self, mode):
    if mode == "force_control":
        self.enable_force_control()
        self.set_force_limits(MAX_FORCE_THRESHOLD)
    elif mode == "impedance_control":
        self.enable_impedance_control()
        self.set_impedance_parameters()
    # ... 기타 모드들

3.2 페달 제어 수정

협동로봇용 페달 매핑:

def pedal_cb_collaborative(self, pedal_real_values):
    if self.teleop_mode == "arm":
        # 협동로봇 특화 제어
        values = self.process_pedal_input(pedal_real_values)
        
        # 힘 제어 모드
        if self.collaborative_mode == "force_control":
            force_x = values["force_x"] * MAX_FORCE_THRESHOLD
            force_y = values["force_y"] * MAX_FORCE_THRESHOLD
            force_z = values["force_z"] * MAX_FORCE_THRESHOLD
            self.on_pub_force_command(force_x, force_y, force_z)
        
        # 임피던스 제어 모드
        elif self.collaborative_mode == "impedance_control":
            stiffness = values["stiffness"] * MAX_STIFFNESS
            damping = values["damping"] * MAX_DAMPING
            self.on_pub_impedance_command(stiffness, damping)

4. 센서 및 피드백 시스템

4.1 협동로봇 센서 통합

class CollaborativeRobotSensors:
    def __init__(self):
        self.force_sensor = None
        self.torque_sensor = None
        self.proximity_sensor = None
        self.safety_light_curtain = None
    
    def get_force_feedback(self):
        # 6축 힘/토크 센서 데이터
        return {
            "fx": self.force_sensor.get_force_x(),
            "fy": self.force_sensor.get_force_y(), 
            "fz": self.force_sensor.get_force_z(),
            "tx": self.torque_sensor.get_torque_x(),
            "ty": self.torque_sensor.get_torque_y(),
            "tz": self.torque_sensor.get_torque_z()
        }
    
    def get_proximity_data(self):
        # 근접 센서 데이터
        return {
            "distance": self.proximity_sensor.get_distance(),
            "direction": self.proximity_sensor.get_direction(),
            "safety_zone": self.safety_light_curtain.get_status()
        }

4.2 안전 피드백 시스템

def safety_feedback_cb(self, sensor_data):
    # 실시간 안전 상태 모니터링
    force_data = sensor_data.get("force")
    proximity_data = sensor_data.get("proximity")
    
    # 위험 상황 감지
    if self.detect_dangerous_condition(force_data, proximity_data):
        self.trigger_emergency_stop()
        self.webserver.control_datachannel_log("EMERGENCY STOP: Safety limit exceeded")
    
    # 사용자에게 안전 상태 전달
    safety_status = self.calculate_safety_status(force_data, proximity_data)
    self.webserver.control_datachannel_log(f"Safety Status: {safety_status}")

5. ArUco 마커 시스템 수정

5.1 협동로봇용 마커 배치

협동로봇의 유연한 작업 공간을 고려한 마커 배치:

# astra_teleop/process.py 수정
def get_collaborative_robot_marker_positions(self):
    # 협동로봇의 더 넓은 작업 공간에 맞는 마커 배치
    obj_points_map_collaborative = {
        # 더 넓은 범위의 마커 배치
        0: [(-20.00, -20.00, 60.00), (-20.00, 20.00, 60.00), (20.00, 20.00, 60.00), (20.00, -20.00, 60.00)],
        # ... 협동로봇 작업 공간에 맞는 마커들
    }
    return obj_points_map_collaborative

5.2 동적 마커 추적

def adaptive_marker_tracking(self, camera_matrix, distortion_coefficients, corners, ids):
    # 협동로봇의 동적 움직임에 대응하는 적응형 마커 추적
    if self.collaborative_mode == "manual_guidance":
        # 수동 가이던스 모드에서는 마커 추적을 보조적으로 사용
        return self.get_manual_guidance_pose()
    else:
        # 일반적인 마커 기반 추적
        return self.solve(camera_matrix, distortion_coefficients, corners, ids)

6. 웹 인터페이스 수정

6.1 협동로봇 전용 UI 요소

// static/index.html에 추가할 협동로봇 UI
const collaborativeRobotUI = {
    safetyStatus: document.getElementById('safety-status'),
    forceDisplay: document.getElementById('force-display'),
    modeSelector: document.getElementById('collaborative-mode'),
    emergencyStop: document.getElementById('emergency-stop-btn'),
    
    updateSafetyStatus(status) {
        this.safetyStatus.textContent = status;
        this.safetyStatus.className = `safety-${status.toLowerCase()}`;
    },
    
    updateForceDisplay(forceData) {
        this.forceDisplay.innerHTML = `
            Fx: ${forceData.fx.toFixed(2)}N
            Fy: ${forceData.fy.toFixed(2)}N  
            Fz: ${forceData.fz.toFixed(2)}N
        `;
    },
    
    enableEmergencyStop() {
        this.emergencyStop.addEventListener('click', () => {
            this.sendControlCommand('emergency_stop');
        });
    }
};

6.2 실시간 안전 모니터링

// 실시간 안전 상태 업데이트
function updateSafetyMonitoring() {
    setInterval(() => {
        fetch('/api/safety-status')
            .then(response => response.json())
            .then(data => {
                collaborativeRobotUI.updateSafetyStatus(data.status);
                collaborativeRobotUI.updateForceDisplay(data.force);
                
                if (data.status === 'DANGER') {
                    collaborativeRobotUI.emergencyStop.style.backgroundColor = 'red';
                }
            });
    }, 100); // 10Hz 업데이트
}

7. 하드웨어 인터페이스 수정

7.1 협동로봇 제어기 통신

# astra_controller/astra_controller/collaborative_robot_controller.py
class CollaborativeRobotController:
    def __init__(self, robot_ip, robot_port):
        self.robot_ip = robot_ip
        self.robot_port = robot_port
        self.safety_monitor = CollaborativeRobotSafety()
        self.sensors = CollaborativeRobotSensors()
    
    def send_position_command(self, joint_positions):
        # 협동로봇 제어기로 위치 명령 전송
        command = {
            "type": "position_control",
            "joints": joint_positions,
            "velocity": self.calculate_safe_velocity(),
            "acceleration": self.calculate_safe_acceleration()
        }
        return self.send_command(command)
    
    def send_force_command(self, force_vector):
        # 힘 제어 명령 전송
        command = {
            "type": "force_control", 
            "force": force_vector,
            "max_force": MAX_FORCE_THRESHOLD
        }
        return self.send_command(command)
    
    def emergency_stop(self):
        # 긴급 정지 명령
        command = {
            "type": "emergency_stop",
            "timestamp": time.time()
        }
        return self.send_command(command)

8. 수정 우선순위 (협동로봇 전환)

8.1 높은 우선순위 (안전 관련)

1. 안전 시스템 통합: 힘/토크 센서, 근접 센서, 긴급 정지
2. 속도 제한 조정: 협동로봇 안전 속도로 변경
3. 기구학 수정: 6축 자유도에 맞는 제어 로직
4. URDF 업데이트: 협동로봇 모델로 변경

8.2 중간 우선순위 (기능 관련)

1. 제어 모드 추가: 힘 제어, 임피던스 제어, 수동 가이던스
2. 센서 피드백: 실시간 힘/토크 모니터링
3. UI 개선: 안전 상태 표시, 긴급 정지 버튼
4. ArUco 마커: 협동로봇 작업 공간에 맞는 배치

8.3 낮은 우선순위 (최적화)

1. 성능 최적화: 실시간 제어 루프 최적화
2. 사용자 경험: 직관적인 협동로봇 인터페이스
3. 로깅 시스템: 안전 이벤트 로깅

시스템 구조 분석

1. 핵심 컴포넌트

1.1 Teleopoperator (teleoprator.py)

• 역할: 웹 클라이언트로부터 받은 입력을 로봇 명령으로 변환
• 주요 기능:
  • ArUco 마커 기반 손 위치 추적
  • 페달 입력 처리 (그리퍼, 리프트, 베이스 제어)
  • 제어 모드 관리 (arm, base, none)

1.2 WebServer (webserver.py)

• 역할: 웹 클라이언트와의 WebRTC 통신 관리
• 주요 기능:
  • 비디오 스트림 제공 (head, wrist_left, wrist_right)
  • 데이터 채널 관리 (hand, pedal, control)

1.3 ArUco 마커 처리 (astra_teleop/process.py)

• 역할: 카메라에서 ArUco 마커를 감지하고 3D 위치 계산
• 주요 기능: get_solve() 함수로 마커 좌표를 로봇 좌표계로 변환

수정이 필요한 부분들

1. ArUco 마커 좌표계 (가장 중요)

1.1 astra_teleop/process.py의 obj_points_map

현재 상태:

obj_points_map = { # right part
    # top_left, top_right, bottom_right, bottom_left
    0:  [ (-15.00, -15.00,  48.28), (-15.00,  15.00,  48.28), ( 15.00,  15.00,  48.28), ( 15.00, -15.00,  48.28) ],
    # ... 더 많은 마커 좌표들
}

수정 필요 사항:

• 마커 위치: 로봇팔 크기가 변경되면 ArUco 마커들의 실제 3D 좌표가 변경됨
• 스케일 팩터: get_solve(scale=1.0)의 스케일 값 조정 필요
• 좌표계 기준점: 로봇팔의 새로운 기하학적 중심점에 맞춰 좌표계 재정의

수정 방법:

1. 새로운 로봇팔에서 ArUco 마커들의 실제 3D 위치 측정
2. obj_points_map의 모든 좌표값 업데이트
3. 필요시 scale 파라미터 조정

2. 로봇팔 물리적 파라미터

2.1 teleoprator.py의 상수값들

현재 상태:

GRIPPER_MAX = 0.055  # 그리퍼 최대 개방 거리 (미터)
INITIAL_LIFT_DISTANCE = 0.8  # 초기 리프트 높이 (미터)
FAR_SEEING_HEAD_TILT = 0.26  # 원거리 시야 헤드 틸트 (라디안)

수정 필요 사항:

• GRIPPER_MAX: 새로운 그리퍼의 최대 개방 거리
• INITIAL_LIFT_DISTANCE: 새로운 로봇팔의 적절한 작업 높이
• LIFT_DISTANCE_MIN/MAX: 리프트 범위 조정
• LIFT_VEL_MAX: 리프트 속도 조정

2.2 헤드 틸트 계산 함수

현재 상태:

def get_head_tilt(self, lift_distance):
    point0_lift = 0
    point0_tilt = 1.36
    point1_lift = 0.8
    point1_tilt = 1.06
    
    return point0_tilt + (point1_tilt - point0_tilt) * (lift_distance - point0_lift) / (point1_lift - point0_lift)

수정 필요 사항:

• 새로운 로봇팔의 작업 공간에 맞춰 헤드 틸트 각도 조정
• 카메라 시야각과 로봇팔 작업 영역의 최적화

3. 리셋 포즈 설정

3.1 reset_arm() 함수의 초기 자세

현재 상태:

goal_pose = {
    "left": self.on_get_initial_eef_pose("left", [self.lift_distance, joint_bent, -joint_bent, 0, 0, 0]),
    "right": self.on_get_initial_eef_pose("right", [self.lift_distance, -joint_bent, joint_bent, 0, 0, 0]),
}

수정 필요 사항:

• 새로운 로봇팔의 기하학에 맞는 초기 자세 설정
• joint_bent 값 조정 (현재 π/4)
• 좌우 팔의 대칭성 유지

4. 웹 인터페이스 관련

4.1 비디오 스트림 설정

현재 상태 (webserver.py):

image_height = 360
image_width = 640
frames_per_second = 30

수정 필요 사항:

• 카메라 해상도가 로봇팔 크기에 적합한지 확인
• 필요시 해상도 조정

4.2 ArUco 마커 감지 설정

현재 상태:

min_aruco_thres=4  # 최소 감지 마커 수

수정 필요 사항:

• 새로운 마커 배치에 따른 최적 마커 수 조정
• 마커 크기와 카메라 해상도의 관계 검토

5. 하드웨어 제어 관련

5.1 astra_controller 패키지

수정 필요 사항:

• 그리퍼 기어 비율: GRIPPER_GEAR_R = 0.027 / 2
• 리프트 레일 파라미터: RAIL_MM_PER_REV = 75, RAIL_MAX_LENGTH_MM = 1190
• 스테퍼 모터 설정: STEPPER_PULSE_PER_REV = 800

5.2 URDF 및 MoveIt 설정

수정 필요 사항:

• astra_description 패키지의 URDF 파일들
• astra_moveit_config 패키지의 MoveIt 설정
• 로봇팔의 새로운 기하학적 파라미터 반영

수정 우선순위

1. 높은 우선순위 (필수)

1. ArUco 마커 좌표계 재정의 (astra_teleop/process.py)
2. 그리퍼 최대 개방 거리 (GRIPPER_MAX)
3. 초기 리프트 높이 (INITIAL_LIFT_DISTANCE)
4. URDF 파일 업데이트

2. 중간 우선순위 (권장)

1. 헤드 틸트 계산 함수 조정
2. 리프트 범위 및 속도 조정
3. 초기 자세 설정 최적화
4. 하드웨어 제어 파라미터 업데이트

3. 낮은 우선순위 (선택)

1. 비디오 해상도 조정
2. ArUco 감지 파라미터 최적화
3. UI 개선 (필요시)

테스트 방법

1. ArUco 마커 시스템 테스트

# astra_teleop/process.py의 get_solve 함수 테스트
from astra_teleop.process import get_solve
solve = get_solve(scale=1.0)
# 실제 카메라로 마커 감지 테스트

2. 텔레오퍼레이션 시스템 테스트

# teleoprator.py의 핵심 기능들 테스트
# - hand_cb: 마커 기반 손 추적
# - pedal_cb: 그리퍼 및 리프트 제어
# - control_cb: 모드 전환

3. 통합 테스트

• 웹 인터페이스를 통한 전체 시스템 테스트
• 실제 로봇팔과의 연동 테스트

주의사항

1. 좌표계 일관성: 모든 좌표계가 동일한 기준점을 사용하는지 확인
2. 단위 통일: 미터, 라디안 등 단위의 일관성 유지
3. 안전성: 새로운 파라미터가 안전한 범위 내에 있는지 확인
4. 백업: 수정 전 기존 설정의 백업 필수

결론

로봇팔의 크기와 형태 변경 시, 가장 중요한 것은 ArUco 마커 좌표계의 재정의입니다. 이는 텔레오퍼레이션 시스템의 핵심인 손 위치 추적의 정확도에 직접적인 영향을 미칩니다. 그 다음으로는 로봇팔의 물리적 파라미터들(그리퍼, 리프트 등)을 새로운 기하학에 맞게 조정해야 합니다.

SCARA에서 협동로봇으로의 전환 시에는 추가적으로 안전 시스템 통합, 속도 제한 조정, 그리고 협동로봇 특화 제어 모드들의 구현이 필수적입니다.

원격조종 시스템 관점에서는 6축 SCARA에서 자유6축 로봇팔로의 전환이 추상화된 인터페이스를 사용하므로 텔레오퍼레이션 코드 자체는 수정할 필요가 없습니다.

이러한 수정을 통해 새로운 로봇팔에서도 정확하고 안전한 텔레오퍼레이션이 가능해집니다.