직접 궤적 전송
MoveIt 없이 Python에서 FollowJointTrajectory 액션으로 직접 궤적을 전송하는 방법을 설명합니다.
사전 조건
plem_launch.py가 실행 중이어야 합니다.- 로봇이 TRAJECTORY 모드여야 합니다 (모드 제어 참조).
액션 인터페이스
- 액션 서버:
/{robot_id}/joint_trajectory_controller/follow_joint_trajectory - 타입:
control_msgs/action/FollowJointTrajectory
1. Joint 이름 확인
궤적을 보내기 전에 joint 이름과 현재 위치를 확인합니다:
ros2 topic echo /indy/joint_states --once
name:
- joint0
- joint1
- joint2
- joint3
- joint4
- joint5
position:
- 0.0
- -0.5236
- 1.5708
- 0.0
- 0.7854
- 0.0
6-DOF 로봇(Indy7, Indy12) 모두 joint0 ~ joint5를 사용합니다.
2. 단일 Waypoint 예제
현재 위치에서 하나의 목표 위치로 이동합니다.
#!/usr/bin/env python3
"""단일 waypoint 궤적 전송 예제."""
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from action_msgs.msg import GoalStatus
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from builtin_interfaces.msg import Duration
JOINT_NAMES = [
"joint0", "joint1", "joint2",
"joint3", "joint4", "joint5",
]
def main():
rclpy.init()
node = Node("single_waypoint_example")
client = ActionClient(
node,
FollowJointTrajectory,
"/indy/joint_trajectory_controller/follow_joint_trajectory",
)
if not client.wait_for_server(timeout_sec=5.0):
node.get_logger().error("액션 서버 연결 실패")
return
# 궤적 구성: 단일 waypoint
trajectory = JointTrajectory()
trajectory.joint_names = JOINT_NAMES
point = JointTrajectoryPoint()
point.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
point.time_from_start = Duration(sec=3)
trajectory.points.append(point)
goal = FollowJointTrajectory.Goal()
goal.trajectory = trajectory
future = client.send_goal(goal)
if future.status == GoalStatus.STATUS_SUCCEEDED:
node.get_logger().info("궤적 실행 완료")
else:
node.get_logger().error(f"궤적 실행 실패: status={future.status}")
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
3. 다중 Waypoint 궤적 예제
여러 웨이포인트를 순서대로 거치는 궤적을 전송합니다.
#!/usr/bin/env python3
"""다중 waypoint 궤적 전송 예제."""
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from action_msgs.msg import GoalStatus
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from builtin_interfaces.msg import Duration
JOINT_NAMES = [
"joint0", "joint1", "joint2",
"joint3", "joint4", "joint5",
]
def build_trajectory(
waypoints: list[tuple[list[float], int]],
) -> JointTrajectory:
"""(positions, time_sec) 리스트로 JointTrajectory를 구성한다."""
trajectory = JointTrajectory()
trajectory.joint_names = JOINT_NAMES
for positions, time_sec in waypoints:
point = JointTrajectoryPoint()
point.positions = positions
point.time_from_start = Duration(sec=time_sec)
trajectory.points.append(point)
return trajectory
def main():
rclpy.init()
node = Node("multi_waypoint_example")
client = ActionClient(
node,
FollowJointTrajectory,
"/indy/joint_trajectory_controller/follow_joint_trajectory",
)
if not client.wait_for_server(timeout_sec=5.0):
node.get_logger().error("액션 서버 연결 실패")
return
# 3개 waypoint: 홈 → 위치A → 위치B
trajectory = build_trajectory([
([0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 3), # 홈 (3초)
([0.5, -0.3, 1.0, 0.0, 0.5, 0.0], 6), # 위치 A (6초)
([0.0, -0.5, 1.2, 0.3, 0.8, -0.2], 10), # 위치 B (10초)
])
goal = FollowJointTrajectory.Goal()
goal.trajectory = trajectory
future = client.send_goal(goal)
if future.status == GoalStatus.STATUS_SUCCEEDED:
node.get_logger().info("궤적 실행 완료")
else:
node.get_logger().error(f"궤적 실행 실패: status={future.status}")
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()
4. time_from_start 설정 가이드
time_from_start는 궤적 시작 시점부터 해당 웨이포인트에 도달해야 하는 시간입니다.
| 규칙 | 설명 |
|---|---|
| 단조 증가 | 각 waypoint의 time_from_start는 이전 waypoint보다 커야 합니다 |
| 충분한 시간 확보 | 너무 짧으면 속도/가속도 제한을 초과하여 목표가 거부됩니다 |
| 첫 waypoint | 현재 위치와 가까우면 1 |
| waypoint 간격 | 관절 이동량에 비례하여 설정. 보수적으로 시작하세요 |
caution
time_from_start가 너무 짧으면 controller가 목표를 거부합니다. 처음에는 넉넉한 시간(5~10초)으로 시작하고, 점진적으로 줄여가세요.
시간이 부족할 때 나타나는 증상
- 목표가 즉시 거부됨 (goal rejected)
- 궤적 실행 중 abort 발생
- 로그에 속도/가속도 제한 초과 메시지
다음 단계
그리퍼 제어로 이동하여 그리퍼를 제어하는 방법을 확인하세요.