첫 궤적 전송

로봇이 구동 중이라면 궤적을 전송하여 움직여 봅니다.

---

전체 흐름

BRAKED → (SetMode) → TRAJECTORY → (FollowJointTrajectory) → 모션 실행 → (SetMode) → BRAKED

1. TRAJECTORY 모드로 전환
2. 궤적 전송
3. BRAKED 모드로 복귀

---

Step 1: TRAJECTORY 모드로 전환

로봇은 기본적으로 BRAKED 모드(mode: 0)로 시작합니다. 궤적을 실행하려면 TRAJECTORY 모드(mode: 2)로 전환해야 합니다.

ros2 action send_goal /indy/wim_rt_driver/set_mode \
  neuromeka_msgs/action/SetMode \
  "{target_robot_mode: {mode: 2}}"

모드	값	설명
BRAKED	0	브레이크 잠금 (기본 상태)
TRAJECTORY	2	궤적 실행 가능
FREEDRIVE	3	수동 조작 가능

warning

TRAJECTORY 모드에서는 로봇이 명령에 따라 움직입니다. 로봇 주변에 사람이나 장애물이 없는지 반드시 확인하세요.

---

Step 2: 현재 Joint 위치 확인

궤적을 보내기 전에 현재 관절 위치를 확인합니다:

ros2 topic echo /indy/joint_states --once

출력 예시:

header:
  stamp:
    sec: 1234567890
    nanosec: 123456789
name:
- joint0
- joint1
- joint2
- joint3
- joint4
- joint5
position:
- 0.0
- -0.5236
- 1.5708
- 0.0
- 0.7854
- 0.0

Joint 이름은 joint0 ~ joint5이며, 6-DOF 로봇에서 모두 동일합니다 (Indy7, Indy12).

---

Step 3: CLI로 궤적 전송

FollowJointTrajectory 액션으로 간단한 궤적을 전송합니다:

ros2 action send_goal \
  /indy/joint_trajectory_controller/follow_joint_trajectory \
  control_msgs/action/FollowJointTrajectory \
  "{trajectory: {
    joint_names: ['joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5'],
    points: [
      {positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], time_from_start: {sec: 3}},
      {positions: [0.5, -0.3, 1.0, 0.0, 0.5, 0.0], time_from_start: {sec: 6}}
    ]
  }}"

• joint_names: 6개 관절 이름 (순서 중요)
• points: 웨이포인트 배열
  • positions: 각 관절의 목표 위치 (radian)
  • time_from_start: 시작 시점부터 해당 웨이포인트 도달까지의 시간

caution

positions 값은 로봇의 관절 범위 내여야 합니다. 범위를 벗어나면 목표가 거부됩니다. 처음에는 작은 값(0.1~0.5 rad)으로 테스트하세요.

---

Step 4: Python 예제

완전한 실행 가능 스크립트입니다. 모드 전환부터 궤적 실행까지 전체 흐름을 포함합니다.

#!/usr/bin/env python3
"""PLEM 궤적 전송 예제 — 모드 전환 → 궤적 실행 → 모드 복귀."""

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 neuromeka_msgs.action import SetMode
from builtin_interfaces.msg import Duration


class PlemTrajectoryExample(Node):
    """모드 전환 → 궤적 전송 → 모드 복귀 예제 노드."""

    # 로봇 모드 상수
    MODE_BRAKED = 0
    MODE_TRAJECTORY = 2

    def __init__(self, robot_id: str = "indy"):
        super().__init__("plem_trajectory_example")
        self._robot_id = robot_id

        # 액션 클라이언트 생성
        self._mode_client = ActionClient(
            self,
            SetMode,
            f"/{robot_id}/wim_rt_driver/set_mode",
        )
        self._traj_client = ActionClient(
            self,
            FollowJointTrajectory,
            f"/{robot_id}/joint_trajectory_controller/follow_joint_trajectory",
        )

    def set_mode(self, mode: int) -> bool:
        """로봇 모드를 변경하고 완료까지 대기한다."""
        self.get_logger().info(f"모드 전환 요청: {mode}")

        if not self._mode_client.wait_for_server(timeout_sec=5.0):
            self.get_logger().error("SetMode 액션 서버 연결 실패")
            return False

        goal = SetMode.Goal()
        goal.target_robot_mode.mode = mode

        future = self._mode_client.send_goal(goal)
        if future.status != GoalStatus.STATUS_SUCCEEDED:
            self.get_logger().error(f"모드 전환 실패: status={future.status}")
            return False

        self.get_logger().info(f"모드 전환 완료: {mode}")
        return True

    def send_trajectory(
        self,
        positions_list: list[list[float]],
        durations_sec: list[int],
    ) -> bool:
        """궤적을 전송하고 완료까지 대기한다."""
        self.get_logger().info(
            f"{len(positions_list)}개 웨이포인트 궤적 전송"
        )

        if not self._traj_client.wait_for_server(timeout_sec=5.0):
            self.get_logger().error("FollowJointTrajectory 액션 서버 연결 실패")
            return False

        # 궤적 메시지 구성
        trajectory = JointTrajectory()
        trajectory.joint_names = [
            "joint0", "joint1", "joint2",
            "joint3", "joint4", "joint5",
        ]
        for positions, sec in zip(positions_list, durations_sec):
            point = JointTrajectoryPoint()
            point.positions = positions
            point.time_from_start = Duration(sec=sec)
            trajectory.points.append(point)

        goal = FollowJointTrajectory.Goal()
        goal.trajectory = trajectory

        future = self._traj_client.send_goal(goal)
        if future.status != GoalStatus.STATUS_SUCCEEDED:
            self.get_logger().error(
                f"궤적 실행 실패: status={future.status}"
            )
            return False

        self.get_logger().info("궤적 실행 완료")
        return True


def main():
    rclpy.init()
    node = PlemTrajectoryExample(robot_id="indy")

    try:
        # 1. TRAJECTORY 모드로 전환
        if not node.set_mode(PlemTrajectoryExample.MODE_TRAJECTORY):
            return

        # 2. 궤적 전송 (2개 웨이포인트)
        node.send_trajectory(
            positions_list=[
                [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],       # 홈 위치 (3초)
                [0.5, -0.3, 1.0, 0.0, 0.5, 0.0],       # 목표 위치 (6초)
            ],
            durations_sec=[3, 6],
        )

        # 3. BRAKED 모드로 복귀
        node.set_mode(PlemTrajectoryExample.MODE_BRAKED)

    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()

실행

# 별도 터미널에서 (plem_launch.py가 실행 중인 상태)
python3 send_trajectory.py

---

Step 5: BRAKED 모드로 복귀

작업이 끝나면 반드시 BRAKED 모드로 돌아갑니다:

ros2 action send_goal /indy/wim_rt_driver/set_mode \
  neuromeka_msgs/action/SetMode \
  "{target_robot_mode: {mode: 0}}"

danger

작업 종료 후 BRAKED 모드로 복귀하지 않으면 로봇이 예기치 않은 명령에 반응할 수 있습니다. 항상 BRAKED 모드로 돌아오세요.

---

문제 해결

"목표 거부됨"

• 로봇이 TRAJECTORY 모드인지 확인하세요.
• joint_names가 정확히 6개이고 이름이 올바른지 확인하세요 (joint0 ~ joint5).
• positions 값이 관절 범위 내인지 확인하세요.

"액션 서버 연결 실패"

• plem_launch.py가 실행 중인지 확인하세요.
• ros2 action list로 액션 서버 목록을 확인하세요.

로봇이 움직이지 않음

• ros2 topic echo /indy/joint_states --once로 joint state가 갱신되는지 확인하세요.
• time_from_start가 충분한지 확인하세요 (너무 짧으면 속도 제한에 걸림).

---

다음 단계

프로젝트 생성으로 이동하여 자체 ROS2 패키지를 만들어 보세요.