이번 작업에서는 MoveIt 2를 설치하고 SO-ARM-101에 대한 모션 플래닝 환경을 구성한 뒤, Unreal에서 목표 포즈를 보내면 MoveIt이 경로를 계획하고 실물 로봇이 실행하는 파이프라인을 만드는 것이 목표이다.
1단계: MoveIt 2 설치 및 환경 검증
WSL2 터미널에서 MoveIt 설치를 진행한다.
sudo apt install -y ros-humble-moveit
설치 완료 후 확인
ros2 pkg list | grep moveit

2단계: MoveIt 설정 패키지 생성
MoveIt에는 MoveIt Setup Assistant를 사용하는 방법과 수동으로 config 패키지를 만드는 방법이있다.
Setup Assistant를 먼저 띄워서 SRDF(self-collision matrix, planning group, end effector, named poses)를 생성하고, controller 설정은 이후에 결정 후 수동 조정하는 방식이 가장 효율적으로 판단된다.
Setup Assistant를 실행
# ROS2 공통 환경변수 설정 후
source /opt/ros/humble/setup.bash
source ~/UnrealRobotics/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export ROS_LOCALHOST_ONLY=1
ros2 launch moveit_setup_assistant setup_assistant.launch.py

[Create New MoveIt Configuration Package] 누르고, .urdf 파일 선택, 우측 하단 [Load Files] 누르기

[Self-Collisions] 탭으로 이동해서 [Generate Collision Matrix] 버튼 클릭

arm 그룹 생성
[Planning Groups] 탭으로 이동해서 [Add Group]으로 그룹 추가
[Group Name] - arm
[Kinematic Solver] - [ kdl_kinematics_plugin/KDLKinematicsPlugin] 선택
[Add Joints]에서 5개의 조인트 오른쪽으로 옮기기
- shoulder_pan
- shoulder_lift
- elbow_flex
- wrist_flex
- wrist_roll

gripper 그룹 생성
다시 [Add Group] 클릭
[Group Name] - gripper
[Kinematic Solver] - [None] (gripper는 IK가 필요 없음)
[Add Joints]에서 gripper 조인트 1개만 선택


home 포즈
[Robot Poses] 탭으로 이동
[Add Pose] 누르고 설정, home 포즈는 모든 조인트를 0으로 설정한 포즈로 설정했다.

ready 포즈
shoulder_lift와 elbow_flex를 약간 움직인 후 저장한다.
참고로 포즈는 나중에 수정 가능하다.

다음으로 [End Effectors] 탭으로 넘어간다.
[Add End Effector] 버튼 클릭
[End Effector Name] - gripper
[End Effector Group] - gripper 선택 (이전에 만들었던 그룹)
[Parent Link] - gripper_link 선택 (arm 체인의 마지막 링크)
[Parent Group] - arm 선택

[Configuration Files] 탭 이동
Output path 설정 후 [Generate Package] 클릭

이제 생성된 패키지를 빌드해서 확인해본다.
cd ~/UnrealRobotics
colcon build --packages-select so101_moveit_config --symlink-install
source install/setup.bash

author 정보가 비어있어서 에러가 났다.
package.xml에 이메일 정보를 넣고 다시 빌드
sed -i 's/<maintainer email="">/<maintainer email="user@example.com">user/' ~/UnrealRobotics/src/so101_moveit_config/package.xml
sed -i 's/<author email="">/<author email="user@example.com">user/' ~/UnrealRobotics/src/so101_moveit_config/package.xml
cd ~/UnrealRobotics
colcon build --packages-select so101_moveit_config --symlink-install

빌드 성공
3단계: ros2_control 인터페이스 결정 및 구현
MoveIt이 로봇을 움직이려면 FollowJointTrajectory action server가 필요한데, 현재 우리 아키텍처에는 이게 없다.
두 가지의 옵션이 있다.
옵션 A: 기존 rclpy bridge에 action server 추가
|
|
본 프로젝트 목표는 디지털 트윈이고, SO-ARM-101 수준의 서보에서 정밀한 trajectory following보다는 목표 위치에 도달이 중요하다는 판단으로 옵션 A로 결정. 나중에 정밀도가 필요하면 옵션 B로 마이그레이션 할 수 있다.
MoveIt이 로봇을 제어하려면 controller 설정 파일과 FollowJointTrajectory action server가 필요하다.
- moveit_controllers.yaml — MoveIt에게 어떤 controller가 있는지 알려주는 설정 파일
- joint_trajectory_action_server.py — MoveIt의 trajectory 명령을 받아 ZMQ로 worker에 전달하는 action server
기존 lerobot_worker.py와 ros_bridge_node.py는 수정하지 않는다. action server가 기존 ZMQ REQ/REP 프로토콜(send_follower_action 명령)을 그대로 사용하되, trajectory의 각 waypoint를 시간 순서대로 전달하는 방식.
단, worker의 2중 안전 클램프 중 --cmd-limit 안전 클램프가 기본 5°로 설정되어 있어서 MoveIt 경로가 클램핑될 수 있다. MoveIt 사용 시에는 --cmd-joints all --cmd-limit 120 정도로 넓혀서 1차 안전 클램프를 무시할 수 있도록 해야한다. 이건 이후 실행 단계에서 설정.
moveit_controllers.yaml 배치
cat > ~/UnrealRobotics/src/so101_moveit_config/config/moveit_controllers.yaml << 'EOF'
# MoveIt controller configuration for SO-ARM-101
# Uses a simple FollowJointTrajectory action server (joint_trajectory_action_server.py)
# that forwards waypoints to lerobot_worker via ZMQ.
moveit_simple_controller_manager:
controller_names:
- arm_controller
arm_controller:
type: FollowJointTrajectory
action_ns: follow_joint_trajectory
default: true
joints:
- shoulder_pan
- shoulder_lift
- elbow_flex
- wrist_flex
- wrist_roll
EOF
demo.launch.py 덮어쓰기
cat > ~/UnrealRobotics/src/so101_moveit_config/launch/demo.launch.py << 'PYEOF'
"""
demo.launch.py — MoveIt demo launch for SO-ARM-101
Modified from Setup Assistant default to:
1. Load our custom moveit_controllers.yaml (FollowJointTrajectory via ZMQ)
2. NOT launch robot_state_publisher (we run it separately with bridge data)
3. NOT launch fake controllers (we use the real action server)
Prerequisites (must be running in separate terminals):
- lerobot_worker.py (conda env)
- ros_bridge_node.py (ROS2 terminal)
- robot_state_publisher (ROS2 terminal)
- joint_trajectory_action_server.py (ROS2 terminal)
"""
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder
def generate_launch_description():
moveit_config = (
MoveItConfigsBuilder("so101_follower", package_name="so101_moveit_config")
.robot_description(
file_path=os.path.join(
os.path.dirname(os.path.dirname(os.path.abspath(__file__))),
"config", "so101_follower.urdf"
),
)
.robot_description_semantic(file_path="config/so101_follower.srdf")
.robot_description_kinematics(file_path="config/kinematics.yaml")
.joint_limits(file_path="config/joint_limits.yaml")
.trajectory_execution(file_path="config/moveit_controllers.yaml")
.planning_pipelines(pipelines=["ompl"])
.pilz_cartesian_limits(file_path="config/pilz_cartesian_limits.yaml")
.to_moveit_configs()
)
# move_group node
move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
moveit_config.to_dict(),
{"use_sim_time": False},
{
"moveit_controller_manager":
"moveit_simple_controller_manager/MoveItSimpleControllerManager"
},
],
)
# RViz with MoveIt plugin
rviz_config = os.path.join(
os.path.dirname(os.path.dirname(os.path.abspath(__file__))),
"config",
"moveit.rviz",
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
output="log",
arguments=["-d", rviz_config],
parameters=[
moveit_config.to_dict(),
{"use_sim_time": False},
],
)
return LaunchDescription([
move_group_node,
rviz_node,
])
PYEOF
action server 스크립트 배치
cat > ~/UnrealRobotics/src/lerobot_ros2_bridge/scripts/joint_trajectory_action_server.py << 'PYEOF'
#!/usr/bin/env python3
"""
joint_trajectory_action_server.py — FollowJointTrajectory action server
Bridges MoveIt's trajectory commands to lerobot_worker via ZMQ REQ/REP.
Runs in system Python 3.10 with rclpy.
Usage:
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/joint_trajectory_action_server.py
"""
import argparse
import json
import math
import time
import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from sensor_msgs.msg import JointState
import zmq
JOINT_NAMES = [
"shoulder_pan",
"shoulder_lift",
"elbow_flex",
"wrist_flex",
"wrist_roll",
"gripper",
]
def rad2deg(rad: float) -> float:
return rad * 180.0 / math.pi
class JointTrajectoryActionServer(Node):
def __init__(self, args):
super().__init__("joint_trajectory_action_server")
# --- ZMQ REQ to worker ---
self.zmq_ctx = zmq.Context()
self.zmq_req = self.zmq_ctx.socket(zmq.REQ)
self.zmq_req.connect(args.req_addr)
self.zmq_req.setsockopt(zmq.RCVTIMEO, 2000)
self.get_logger().info(f"ZMQ REQ connected to {args.req_addr}")
# --- Subscribe to /joint_states for feedback ---
self.current_positions = {}
self.js_sub = self.create_subscription(
JointState, "/joint_states", self.on_joint_state, 10
)
# --- Action server ---
self._action_server = ActionServer(
self,
FollowJointTrajectory,
"/arm_controller/follow_joint_trajectory",
self.execute_callback,
)
self.get_logger().info(
"FollowJointTrajectory action server ready on "
"/arm_controller/follow_joint_trajectory"
)
def on_joint_state(self, msg: JointState):
for name, pos in zip(msg.name, msg.position):
self.current_positions[name] = pos
def send_to_worker(self, joint_positions_deg: dict) -> dict:
cmd = {
"cmd": "send_follower_action",
"args": joint_positions_deg,
}
try:
self.zmq_req.send_string(json.dumps(cmd))
reply_raw = self.zmq_req.recv_string()
return json.loads(reply_raw)
except zmq.Again:
self.get_logger().error("ZMQ REQ timeout — is lerobot_worker running?")
return {"status": "error", "reason": "zmq_timeout"}
except Exception as e:
self.get_logger().error(f"ZMQ REQ error: {e}")
return {"status": "error", "reason": str(e)}
def execute_callback(self, goal_handle):
trajectory = goal_handle.request.trajectory
joint_names = list(trajectory.joint_names)
points = trajectory.points
self.get_logger().info(
f"Executing trajectory: {len(points)} points, joints: {joint_names}"
)
if len(points) == 0:
goal_handle.succeed()
result = FollowJointTrajectory.Result()
result.error_code = FollowJointTrajectory.Result.SUCCESSFUL
return result
t_start = time.monotonic()
feedback = FollowJointTrajectory.Feedback()
feedback.joint_names = joint_names
for i, point in enumerate(points):
if goal_handle.is_cancel_requested:
goal_handle.canceled()
self.get_logger().info("Trajectory canceled")
result = FollowJointTrajectory.Result()
result.error_code = FollowJointTrajectory.Result.SUCCESSFUL
return result
# Wait until scheduled time
target_time = (
point.time_from_start.sec
+ point.time_from_start.nanosec * 1e-9
)
elapsed = time.monotonic() - t_start
wait_time = target_time - elapsed
if wait_time > 0:
time.sleep(wait_time)
# Convert radians -> degrees, send to worker
joint_positions_deg = {}
for name, pos_rad in zip(joint_names, point.positions):
if name in JOINT_NAMES:
joint_positions_deg[name] = rad2deg(pos_rad)
if not joint_positions_deg:
continue
reply = self.send_to_worker(joint_positions_deg)
if reply.get("status") != "ok":
self.get_logger().warn(
f"Point {i}: worker replied {reply.get('reason', 'unknown')}"
)
# Publish feedback
feedback.desired = point
feedback.actual = JointTrajectoryPoint()
feedback.actual.positions = [
self.current_positions.get(name, 0.0) for name in joint_names
]
feedback.actual.time_from_start = point.time_from_start
goal_handle.publish_feedback(feedback)
if (i + 1) % 10 == 0 or i == len(points) - 1:
self.get_logger().info(
f" Point {i+1}/{len(points)} sent (t={target_time:.2f}s)"
)
goal_handle.succeed()
result = FollowJointTrajectory.Result()
result.error_code = FollowJointTrajectory.Result.SUCCESSFUL
self.get_logger().info("Trajectory execution complete")
return result
def destroy_node(self):
self.get_logger().info("Shutting down ZMQ...")
self.zmq_req.close()
self.zmq_ctx.term()
super().destroy_node()
def main():
parser = argparse.ArgumentParser(
description="FollowJointTrajectory action server (ZMQ bridge)"
)
parser.add_argument(
"--req-addr", default="tcp://127.0.0.1:5556",
help="ZMQ REQ address (worker REP)",
)
args, unknown = parser.parse_known_args()
rclpy.init(args=unknown if unknown else None)
node = JointTrajectoryActionServer(args)
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
PYEOF
파일 실행 권한 부여
chmod +x ~/UnrealRobotics/src/lerobot_ros2_bridge/scripts/joint_trajectory_action_server.py
demo.launch.py가 참조할 URDF를 config 폴더에도 복사
#URDF를 moveit_config/config에 복사 (launch에서 참조)
cp ~/UnrealRobotics/src/so101_description/urdf/legacy/so101_follower.urdf \
~/UnrealRobotics/src/so101_moveit_config/config/so101_follower.urdf
#control_msgs가 설치되어 있는지 확인
ros2 pkg list | grep control_msgs
모든 파일 수정 및 배치했고 빌드해서 확인
cd ~/UnrealRobotics
colcon build --packages-select so101_moveit_config --symlink-install
source install/setup.bash

성공했다면 넘어간다.
4단계: MoveIt demo 동작 검증
이제 전체 파이프라인을 띄워서 테스트해본다. 로봇을 연결한 후 MoveIt + action server가 정상 기동되는지 확인. 총 4개 터미널이 필요하다.
로봇 연결
Attach-Both
터미널 1 - lerobot_worker (conda)
conda activate lerobot
sudo chmod 666 /dev/ttyACM*
cd ~/UnrealRobotics/src/lerobot_ros2_bridge
python scripts/lerobot_worker.py --cmd-joints all --cmd-limit 120
터미널 2 - ros_bridge_node (ROS2)
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 - action server (ROS2)
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/joint_trajectory_action_server.py
터미널 4 - MoveIt demo launch (ROS2)
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 launch so101_moveit_config demo.launch.py

모든 터미널을 열었다면 RViz 화면이 뜬다. 그런데 [Global Status: Warn]이므로 현재 오류가 있는 상태다.

joint_limits.yaml에서 max_velocity와 max_acceleration 값이 정수(10, 0)로 되어 있는데 MoveIt은 double 타입을 요구하기 때문에 오류가 났다.
joint_limits.yaml 수정
sed -i 's/max_velocity: 10$/max_velocity: 10.0/' ~/UnrealRobotics/src/so101_moveit_config/config/joint_limits.yaml
sed -i 's/max_acceleration: 0$/max_acceleration: 0.0/' ~/UnrealRobotics/src/so101_moveit_config/config/joint_limits.yaml
joint_limits.yaml 수정 확인
grep -E "max_velocity|max_acceleration" ~/UnrealRobotics/src/so101_moveit_config/config/joint_limits.yaml

터미널 4 다시 실행
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 launch so101_moveit_config demo.launch.py

여전히 Global Status: Warn 상태이고, 이 문제는 robot_state_publisher가 안 떠 있어서 TF가 없는 것.
demo.launch.py에서 robot_state_publisher를 빼놨기 때문에 별도 터미널에서 띄워야 합니다.
터미널 5 - robot_state_publisher
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
xacro ~/UnrealRobotics/src/so101_description/urdf/so101_arm.urdf.xacro variant:=follower > /tmp/so101_follower.urdf
python3 -c "
import yaml
with open('/tmp/so101_follower.urdf') as f: urdf = f.read()
params = {'robot_state_publisher': {'ros__parameters': {'robot_description': urdf}}}
with open('/tmp/rsp_params.yaml', 'w') as f: yaml.dump(params, f, default_flow_style=False)
"
ros2 run robot_state_publisher robot_state_publisher --ros-args --params-file /tmp/rsp_params.yaml

터미널 5를 실행한 후 다시 RViz를 보면 Global Status: OK, Fixed Frame: OK를 확인할 수 있다.
여기까지 5개의 터미널 모습

MoveIt 플래닝 테스트

[Planning Group]: arm 설정
[Goal State]: ready 설정
설정 후 [Plan] 버튼 누르면 오른쪽에 목표 포즈가 재생된다.

이제 [Execute] 버튼을 누르기 전에 터미널 1(worker), 터미널 3(action server)이 에러 없이 정상 동작 중인지 확인하고, 로봇이 움직일 수 있는 공간이 있는지 확인한다.
이제 [Execute]만 누르면 되는데 다시 보니 Plan이 Failed가 되어서 Execute 버튼이 비활성화 되어 있었다.
터미널 4에서 에러 메시지 확인하니 shoulder_link과 lower_arm_link 사이에서 self-collision이 감직되고 있었다. (계속 이 부분이 불안했던 상황)
이 충돌은 실제로 부딪히는 게 아니라 URDF 메시의 부정확함 때문에 생기는 오탐(false positive). URDF의 zero point와 LeRobot calibration의 zero point가 달라서, MoveIt이 보는 로봇 포즈에서 메시가 겹쳐 보이는 것이지 실물은 겹치지지 않는다.
Setup Assistant에서 "Generate Collision Matrix"를 했을 때 이 쌍이 자동으로 비활성화되지 않은 건, 당시 로봇의 실제 포즈 데이터 없이 URDF home pose(0°)만으로 판단했기 때문.
self-collision을 해결하기 위해 SRDF에서 이 충돌 쌍을 비활성화한다.
# shoulder_link ↔ lower_arm_link 충돌 비활성화 추가
cd ~/UnrealRobotics/src/so101_moveit_config/config
# 기존 SRDF에 충돌 방지 추가
sed -i '/<\/robot>/i \ <disable_collisions link1="shoulder_link" link2="lower_arm_link" reason="Adjacent"/>' so101_follower.srdf
터미널 4 중단 후 다시 실행
ros2 launch so101_moveit_config demo.launch.py

이번에는 Failed가 뜨지 않고 Time이 뜨면서 [Execute] 버튼도 활성화 됐다.
[Execute] 버튼을 누르면, 실제 로봇이 Plan 대로 움직이는 것을 확인 할 수 있다.

모터가 떨리면서 움직이는 이유
떨림의 원인은 MoveIt trajectory의 waypoint를 하나씩 개별 명령으로 보내는 방식 때문
현재 동작 흐름은, MoveIt이 수십 개의 waypoint를 생성하고, action server가 각 waypoint마다 ZMQ REQ → worker →
send_action()을 호출한다.
Velocity Scaling을 0.10에서 0.30~0.50으로 올리면 waypoint 간 시간 간격이 짧아져서 떨림이 줄어들 수 있고,
이후에 서보의 속도/가속도 프로파일을 활용하거나, trajectory를 다운샘플링해서 핵심 waypoint만 보내는 방식으로 개선할 수 있다.
완료한 작업
- MoveIt 2 설치 (ros-humble-moveit, 25개 패키지)
- Setup Assistant로 so101_moveit_config 패키지 생성 (SRDF, kinematics, joint_limits, collision matrix)
- joint_limits.yaml int→double 수정, SRDF에 shoulder_link↔lower_arm_link collision disable 추가
- Controller 방식 결정: ros2_control 대신 FollowJointTrajectory action server(ZMQ 경유) 채택
- joint_trajectory_action_server.py 작성 — MoveIt trajectory waypoint를 ZMQ로 worker에 전달
- demo.launch.py 수정 — moveit_simple_controller_manager 사용, robot_state_publisher 분리
- End-to-end 성공: RViz MotionPlanning → Plan → Execute → 실물 follower arm 동작
- IK(interactive marker) + FK(Joints 슬라이더) 양쪽 방식으로 목표 설정 + 실행 확인
'Unreal Engine > 언리얼-ROS-Physical 통합 프로젝트' 카테고리의 다른 글
| [UnrealRobotics: SO-101] (14) Unreal Engine → MoveIt → 실물 로봇: 양방향 디지털 트윈 End-to-End (0) | 2026.05.13 |
|---|---|
| [UnrealRobotics: SO-101] (12) Unreal에서 ROS2 로봇 명령 송신 파이프 확장 (0) | 2026.05.12 |
| [UnrealRobotics: SO-101] (11) Unreal에 3D 로봇 파트 올리기, 연동 확인 (0) | 2026.05.07 |
| [UnrealRobotics: SO-101] (10) Joint Command 역방향 경로 검증 (ROS → 로봇) (0) | 2026.04.27 |
| [UnrealRobotics: SO-101] (9) 실물 로봇 + RViz 실시간 동기화 (0) | 2026.04.22 |