MuJoCo 시뮬레이션 환경에서 로봇 팔/손 악수 동작을 위한 작업공간 임피던스 제어 구현

MuJoCo 시뮬레이션 환경에서 로봇 팔/손의 악수 동작을 구현할 때 가장 중요한 핵심은 "강체 제어(Position Control)"가 아닌 "유연성 제어(Compliance/Impedance Control)"를 적용하는 것입니다.

MuJoCo는 관절 공간(Joint Space) 및 작업 공간(Task/Operational Space)에서의 토크 제어를 정교하게 계산해 주기 때문에, 작업 공간 임피던스 제어(Task-Space Impedance Control)를 구현하기에 매우 적합합니다.

1. 악수 제어 시스템 구조

악수 동작은 크게 두 가지 영역으로 나누어 제어합니다.

1. 로봇 팔 (Arm - Task-Space Impedance Control)

사람이 팔을 위아래로 흔들거나 끌어당길 때, 질량-스프링-댐퍼 시스템처럼 유연하게 따라오도록 목표 위치 \(x_d\) 근처에서 강성(\(K\))과 감쇠(\(D\))를 조절합니다.

2. 로봇 손/손가락 (Hand - Direct Force/Torque Control)

접촉 센서(Touch/Force Sensor) 피드백을 받아 사람 손이 들어오면 너무 강하지 않은 적정 파지력(Grasping Force)을 유지합니다.

$$ \tau = J(q)^T \Big( M_x (\ddot{x}_d - \ddot{x}) + D_x (\dot{x}_d - \dot{x}) + K_x (x_d - x) \Big) + C(q, \dot{q}) + g(q) $$

Tip: MuJoCo에서는 중력 및 코리올리 힘(\(C(q,\dot{q}) + g(q)\))을 d.qfrc_bias 변수로 이미 계산해주므로, 임피던스 외력 토크에 d.qfrc_bias만 더해주면 간편하게 중력 보상이 포함된 토크 제어를 수행할 수 있습니다.

2. Python (Mujoco Py-binding) 구현 예제 코드

아래 예제는 MuJoCo Python API를 활용하여 작업 공간(Task Space) 임피던스 제어기로 악수를 유연하게 받아주는 기본 컨트롤러 루프 구조입니다.

import mujoco
import numpy as np

# 1. 모델 및 데이터 로드 (로봇 팔 및 손 XML)
model = mujoco.MjModel.from_xml_path("handshake_robot.xml")
data = mujoco.MjData(model)

# 엔드이펙터(손목/손 중심) Body ID 및 Site ID 설정
ee_site_name = "end_effector_site"
ee_site_id = model.site(ee_site_name).id

# 임피던스 제어 게인 설정 (Virtual Stiffness & Damping)
# 악수 시 유연성을 위해 Kx(강성)를 비교적 낮게 설정합니다.
Kx = np.diag([150.0, 150.0, 100.0, 20.0, 20.0, 20.0])  # x, y, z, rx, ry, rz
Dx = np.diag([20.0, 20.0, 15.0, 2.0, 2.0, 2.0])      # Damping

# 목표 악수 위치 (Target Pose)
x_des = np.array([0.5, 0.0, 0.4])  # 사람 손과 만날 목표 위치 (x, y, z)
R_des = np.eye(3)                  # 목표 자세 (Rotation matrix)

def compute_impedance_torque(m, d):
    # 1. 현재 엔드이펙터 위치 및 자세 추출
    x_curr = d.site(ee_site_id).xpos
    R_curr = d.site(ee_site_id).xmat.reshape(3, 3)

    # 2. 위치 및 회전 오차 계산
    pos_err = x_des - x_curr

    # 회전 오차 (Orientation Error via skew-symmetric matrix)
    R_err = R_des @ R_curr.T
    rot_err = 0.5 * np.array([
        R_err[2, 1] - R_err[1, 2],
        R_err[0, 2] - R_err[2, 0],
        R_err[1, 0] - R_err[0, 1]
    ])

    delta_x = np.concatenate([pos_err, rot_err])

    # 3. 자코비안(Jacobian) 계산 (Translational + Rotational)
    jacp = np.zeros((3, m.nv))
    jacr = np.zeros((3, m.nv))
    mujoco.mj_jacSite(m, d, jacp, jacr, ee_site_id)
    J = np.vstack([jacp, jacr])

    # 4. 현재 작업 공간 속도 계산 (dx = J * dq)
    dx = J @ d.qvel

    # 5. 작업 공간 제어력 (Task-Space Force) 계산
    F_task = Kx @ delta_x - Dx @ dx

    # 6. 관절 토크 변환 (Tau = J^T * F + Gravity Compensation)
    tau_impedance = J.T @ F_task
    tau_gravity = d.qfrc_bias  # 중력 및 코리올리 보상 항

    return tau_impedance + tau_gravity

# 메인 제어 루프
while data.time < 10.0:
    # 컨트롤러 토크 계산 및 인가
    tau = compute_impedance_torque(model, data)

    # 관절 구동기(Actuator)에 토크 입력 (Torque Motor 기준)
    data.ctrl[:model.nu] = tau[:model.nu]

    # 손가락 파지 제어 (접촉 센서 피드백에 따른 파지력 제어 예시)
    touch_force = data.sensor("hand_touch_sensor").data[0]
    if touch_force > 0.5:  # 손이 닿으면
        data.ctrl[model.nu-1] = 0.3  # 손가락 살짝 쥐기 (유연한 파지)
    else:
        data.ctrl[model.nu-1] = 0.0  # 대기 상태

    # 시뮬레이션 Step
    mujoco.mj_step(model, data)

3. 핵심 구현 및 시뮬레이션 팁

① MJCF (XML) 설정 시 주의사항

• Actuator 타입: <motor> 태그를 사용하거나, <position> 제어기 사용 시 kp 값을 매우 낮게 잡고 kv(댐핑)를 적절히 배분해야 유연성이 확보됩니다. (토크 제어 모드 추천)

• 손가락 마디 마찰력: 악수 시 손이 미끄러지지 않도록 그리퍼/손가락 파지면의 마찰 계수를 높여줍니다.

<geom name="finger_pad" type="box" friction="1.5 0.005 0.0001" condim="4"/>

② 외력 동기화 (External Force Interaction)

시뮬레이션 상에서 사람이 손을 흔드는 동작을 모사하려면, 엔드이펙터에 주기적인 외력(\(F_{ext} = A \sin(\omega t)\))을 인가하는 방식으로 테스트할 수 있습니다.

# 사람이 팔을 위아래로 흔드는 외력 모사 (d.xfrc_applied 활용)
data.xfrc_applied[ee_body_id] = [0, 0, 5.0 * np.sin(2 * np.pi * 1.0 * data.time), 0, 0, 0]

③ RL(강화학습) 환경 확장 (Isaac Lab / MuJoCo Gymnasium)

RL 기반으로 다양한 사람의 파지/흔듦 패턴에 대응하려면, State에 6D F/T 센서 값과 상대 위치 오차를 포함시키고, Action으로 임피던스 제어기의 Target Pose(\(x_d\)) 또는 Stiffness (\(K_x\))를 직접 출력하게 설계하는 것이 Sim-to-Real 성능에 가장 유리합니다.