직접 궤적 전송

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	현재 위치와 가까우면 12초, 멀면 35초 이상
waypoint 간격	관절 이동량에 비례하여 설정. 보수적으로 시작하세요

주의

time_from_start가 너무 짧으면 controller가 목표를 거부합니다. 처음에는 넉넉한 시간(5~10초)으로 시작하고, 점진적으로 줄여가세요.

시간이 부족할 때 나타나는 증상

• 목표가 즉시 거부됨 (goal rejected)
• 궤적 실행 중 abort 발생
• 로그에 속도/가속도 제한 초과 메시지

---

다음 단계

그리퍼 제어로 이동하여 그리퍼를 제어하는 방법을 확인하세요.