[UnrealRobotics: SO-101] (10) Joint Command 역방향 경로 검증 (ROS → 로봇)

 

 

 

---

 

 

Joint Command 구독 검증

언리얼 → 실물 로봇 제어를 위한 /follower_joint_commands → ZMQ REQ → worker → send_action() 역방향 경로 검증

 

 

1단계: Worker(팔로워암)에 안전 클램핑 추가

Worker 시작 시 follower arm의 초기 위치를 기록하고, 명령은 그 기준 ±5° 범위로 클램핑한다.

 

 

lerobot_worker.py 파일 업데이트

lerobot_worker.py

0.01MB

 

새 CLI 인자 2개:

• --cmd-limit 5.0 — 초기 위치 기준 ±N° 클램핑 (기본 5°)
• --cmd-joints shoulder_pan — 명령 허용 관절 (기본: shoulder_pan만). all로 전체 허용

안전 메커니즘:

• 로봇 연결 직후 follower 초기 위치를 기록하고 허용 범위를 출력
• handle_command에서 허용되지 않은 관절은 거부, 허용된 관절도 baseline ±limit로 클램핑
• 클램핑 발생 시 로그 출력

 

 

test_joint_cmd.py 새 테스트 파일 추가, lerobot_worker.py와 같은 폴더에 배치

test_joint_cmd.py

0.00MB

 

CLI 명령어

 

• python3 test_joint_cmd.py → shoulder_pan에 +3° 단발 명령
• python3 test_joint_cmd.py --offset -3.0 → shoulder_pan에 -3°
• python3 test_joint_cmd.py --sweep → ±3° 사인파 4초 스윕

 

 

---

 

 

2단계: Shoulder_pan 관절 테스트

두 로봇 모두 Attach-Both로 연결 후 WSL로 넘어가서 확인 및 권한 설정

 

 

터미널 1 - Worker

conda activate lerobot
sudo chmod 666 /dev/ttyACM*
cd ~/UnrealRobotics/src/lerobot_ros2_bridge
python scripts/lerobot_worker.py --follower-only

현재 포지션을 기준으로 clamp 테스트 가능한 각도가 출력된다.

 

 

터미널 2 - Bridge node:

export PATH=$(echo $PATH | tr ':' '\n' | grep -v miniforge | tr '\n' ':' | sed 's/:$//')
source /opt/ros/humble/setup.bash
source ~/UnrealRobotics/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export ROS_LOCALHOST_ONLY=1
ros2 run lerobot_ros2_bridge bridge_node

 

 

터미널 3 - 테스트 명령 전송:

export PATH=$(echo $PATH | tr ':' '\n' | grep -v miniforge | tr '\n' ':' | sed 's/:$//')
source /opt/ros/humble/setup.bash
source ~/UnrealRobotics/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export ROS_LOCALHOST_ONLY=1

# test_joint_cmd.py를 적절한 곳에 복사해두고 실행
python3 ~/UnrealRobotics/src/lerobot_ros2_bridge/scripts/test_joint_cmd.py --absolute -2.0

실행하면 Shoulder_pan에 해당하는 관절이 살짝 움직이는 것을 볼 수 있다.

 

 

 

터미널 3 - 연속 움직임 테스트 명령 전송:

python3 ~/UnrealRobotics/src/lerobot_ros2_bridge/scripts/test_joint_cmd.py --sweep --sweep-amp 3.0 --sweep-dur 4.0

좌우를 아주 살짝만 움직인다.

 

 

 

터미널 3 - 클램핑 작동 여부 테스트 명령 전송(클램핑 밖의 값을 전송하여 확인):

python3 ~/UnrealRobotics/src/lerobot_ros2_bridge/scripts/test_joint_cmd.py --absolute 5.0

이미 Shoulder_pan이 0에 가까운 상태여서 로봇은 거의 움직이지 않았고,

 

터미널1에서는 성공적으로 클램핑이 된 것을 확인할 수 있었다.

 

 

---

 

 

3단계: 전체 관절 테스트

lerobot_worker.py 파일 업데이트

lerobot_worker.py

0.01MB

 

 

 

test_joint_cmd.py 파일 업데이트

test_joint_cmd.py

0.01MB

• --sweep-all 모드 추가: 6개 관절 순차 테스트, 관절마다 Enter를 눌러야 진행
• 전송 속도 향상: 10Hz -> 30Hz로 부드러운 동작

 

 

터미널 1 - worker 재시작:

# 기존 Ctrl+C로 종료 후
python scripts/lerobot_worker.py --follower-only --cmd-joints all --cmd-limit 10.0

 

 

터미널 3 - 전 관절 순차 테스트:

python3 ~/UnrealRobotics/src/lerobot_ros2_bridge/scripts/test_joint_cmd.py --sweep-all --sweep-amp 5.0 --sweep-dur 4.0

모든 관절이 현재 위치 기준 +5°를 범위로 왕복하는 것을 볼 수 있다.

 

 

---

 

 

관절 한계 각도 출력

*Lerobot 내부 각도 normalization 공식을 몰라서 각 관절의 전체 범위 각도 출력이 불가능하다. Calibration은 raw 데이터를 받아오는 것이었고, raw 데이터에서 degree로 변환하는 값을 역으로 유추해본다.

터미널1 끄고 실행

cat > /tmp/measure_limits.py << 'PYEOF'
from lerobot.robots.so_follower import SO101Follower, SO101FollowerConfig
import json, pathlib

cfg = SO101FollowerConfig(port="/dev/ttyACM0", id="so101_twin_follower")
robot = SO101Follower(cfg)
robot.connect()

# Disable torque so joints move freely by hand
robot.bus.disable_torque()
print("*** TORQUE DISABLED — joints are now free to move ***")

joints = ["shoulder_pan","shoulder_lift","elbow_flex","wrist_flex","wrist_roll","gripper"]

obs1 = robot.get_observation()
raw1 = robot.bus.sync_read("Present_Position", normalize=False)

print("\n=== Point 1 recorded (current position) ===")
for n in joints:
    print(f"  {n:20s}: {float(obs1[f'{n}.pos']):+8.2f} deg  raw={int(raw1[n])}")

input("\nMove ALL joints a little bit by hand, then press Enter...")

obs2 = robot.get_observation()
raw2 = robot.bus.sync_read("Present_Position", normalize=False)

print("\n=== Point 2 recorded ===")
for n in joints:
    print(f"  {n:20s}: {float(obs2[f'{n}.pos']):+8.2f} deg  raw={int(raw2[n])}")

calib_path = pathlib.Path.home() / ".cache/huggingface/lerobot/calibration/robots/so_follower/so101_twin_follower.json"
with open(calib_path) as f:
    calib = json.load(f)

print("\n=== JOINT LIMITS IN LEROBOT DEGREES ===")
limits = {}
for n in joints:
    d1, d2 = float(obs1[f"{n}.pos"]), float(obs2[f"{n}.pos"])
    r1, r2 = int(raw1[n]), int(raw2[n])
    if r2 == r1:
        print(f"  {n:18s} | Joint didn't move! Skipping.")
        continue
    scale = (d2 - d1) / (r2 - r1)
    offset = d1 - scale * r1
    rmin, rmax = calib[n]["range_min"], calib[n]["range_max"]
    deg_min = scale * rmin + offset
    deg_max = scale * rmax + offset
    lo, hi = min(deg_min, deg_max), max(deg_min, deg_max)
    limits[n] = (round(lo, 2), round(hi, 2))
    print(f"  {n:18s} | {lo:+9.2f} ~ {hi:+9.2f} deg")

print("\n=== Worker에 붙여넣기용 ===")
print("JOINT_LIMITS_DEG = {")
for n, (lo, hi) in limits.items():
    print(f'    "{n}": ({lo}, {hi}),')
print("}")

robot.disconnect()
PYEOF

python /tmp/measure_limits.py

서로 다른 두 포지션의 로봇 관절 위치를 통해서 확실한 한계 각도를 계산했다. 이전에 진행했던 Calibration 값도 쓰였다.

 

파일 업데이트

lerobot_worker.py

0.02MB

 

변경 사항 정리:

• JOINT_LIMITS_DEG: 실측 LeRobot degrees 기준 물리적 한계 추가
• 이중 클램핑 복원: baseline ± cmd_limit AND 실측 physical limits
• baseline 출력: effective range와 physical limits 모두 표시

 

터미널1 실행

python scripts/lerobot_worker.py --follower-only --cmd-joints all --cmd-limit 10.0

 

 

터미널3 실행 (conda deactivate 상태에서)

export PATH=$(echo $PATH | tr ':' '\n' | grep -v miniforge | tr '\n' ':' | sed 's/:$//')
source /opt/ros/humble/setup.bash
source ~/UnrealRobotics/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export ROS_LOCALHOST_ONLY=1

python3 ~/UnrealRobotics/src/lerobot_ros2_bridge/scripts/test_joint_cmd.py --sweep-all --sweep-amp 5.0 --sweep-dur 4.0

 

좌우 10도로 늘려서 보려면

python3 ~/UnrealRobotics/src/lerobot_ros2_bridge/scripts/test_joint_cmd.py --sweep-all --sweep-amp 10.0 --sweep-dur 4.0

 

 

각 관절이 명령에 의해서 움직이는 것을 확인할 수 있다.

 

 

---

 

 

오늘 수행한 작업

• Command 경로 End-to-End 검증
  • /follower_joint_commands 토픽 → bridge node → ZMQ → worker → follower 실물 동작 확인
• Safety Clamp 시스템 구현
  • Worker 시작 시 follower 초기 위치를 기록하여 baseline으로 사용
  • --cmd-limit(기본 ±5°)로 baseline 기준 이동 범위 제한
  • --cmd-joints로 명령 허용 관절을 제한(기본: shoulder_pan만)
  • 범위 초과 시 자동 클램핑 + 로그 출력
• URDF vs LeRobot 좌표계 불일치 발견 및 해결
  • URDF limit을 제거하고, 토크 해제 후 두 포인트 측정 → 선형 보간으로 calibration range를 LeRobot degrees로 변환하여 실측 물리적 한계 확정
• 실측 Joint Limits 확정 (LeRobot degrees 기준)
  • shoulder_pan: ±119.91°
  • shoulder_lift: ±104.62°
  • elbow_flex: ±97.01°
  • wrist_flex: ±102.68°
  • wrist_roll: ±180.00°
  • gripper: +0.14° ~ +99.45°
• 테스트 도구 작성 (test_joint_cmd.py)
  • 단발(--absolute), sweep(--sweep), 전 관절 순차(--sweep-all) 모드
  • /joint_states를 구독하여 현재 위치 기준 baseline-relative sweep
  • --sweep-rate로 전송 주파수 조정 (10Hz→30Hz에서 떨림 해소 확인)
• 전 관절 ±10° 왕복 테스트 성공
  • 6개 관절 모두 정상 동작 확인
  • 이중 클램핑([BASELINE LIMIT] / [PHYSICAL LIMIT]) 정상 작동 확인

 

 

---