Refactor motion sequence handling: remove old test script, update setup, and implement new runner with configuration support

- Deleted obsolete `test_motion_sequence.py` file. - Updated `setup.py` to remove references to the old test script and motion configuration file. - Added `motion_sequence_config.json` to define home position and waypoints for the robot. - Introduced `motion_sequence_runner.py` to execute motion sequences based on the new configuration format, supporting both joint and pose movements.
2026-05-26 16:09:41 +10:00
parent a84cfde96c
commit a4d318fceb
6 changed files with 138 additions and 984 deletions
@@ -201,30 +201,14 @@ ros2 service call /iiwa/stop std_srvs/srv/Trigger "{}"

 Примеры использования `test_motion_sequence`:
 ```bash
-# Просто выполнить последовательность без записи
-ros2 run iiwa_utils test_motion_sequence \
-  --ros-args -p n_iterations:=3 \
-             -p delay_between_iterations:=5.0
-
-# Записать все доступные топики в bag
-ros2 run iiwa_utils test_motion_sequence \
-  --ros-args -p n_iterations:=5 \
+ros2 run iiwa_planning motion_sequence_runner \
+  --ros-args \
+  -p config_path:=/path/to/config.json \
+  -p n_iterations:=3 \
  -p delay_between_iterations:=5.0 \
-             -p bag_path:=/tmp/iiwa_session
-
-# Записать конкретные топики
-ros2 run iiwa_utils test_motion_sequence \
-  --ros-args -p n_iterations:=5 \
-             -p delay_between_iterations:=5.0 \
-             -p bag_path:=/tmp/iiwa_session \
-             -p topics:="['/joint_states', '/d455_top/color/image_raw', '/tf']"
-
-# Использовать свой конфиг поз
-ros2 run iiwa_utils test_motion_sequence \
-  --ros-args -p config_path:=/path/to/my_config.json \
-             -p n_iterations:=1 \
-             -p delay_between_iterations:=3.0 \
-             -p bag_path:=/tmp/iiwa_session
+  -p bag_path:=/tmp/my_bag \
+  -p joints_action:=my_ns/move_to_joints \
+  -p pose_action:=my_ns/move_to_pose

 ```

@@ -43,5 +43,16 @@ install(PROGRAMS
  RENAME move_to_pose_server
 )

+install(PROGRAMS
+  scripts/motion_sequence_runner.py
+  DESTINATION lib/${PROJECT_NAME}
+  RENAME motion_sequence_runner
+)
+
+install(FILES
+  scripts/motion_sequence_config.json
+  DESTINATION share/${PROJECT_NAME}
+)
+

 ament_package()
@@ -0,0 +1,42 @@
+{
+  "home": {
+    "joints": [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0],
+    "speed": 0.1
+  },
+  "waypoints": [
+    {
+      "x": 0.6,
+      "y": 0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 2.7916,
+      "speed": 0.4,
+      "planner": "ptp"
+    },
+    {
+      "x": 0.6,
+      "y": -0.1,
+      "z": 0.55,
+      "a": 3.14,
+      "b": 0.3142,
+      "c": 2.7916,
+      "speed": 0.3,
+      "planner": "lin"
+    },
+    {
+      "joints": [0.5, 0.3, 0.0, -1.2, 0.0, 1.4, 0.0],
+      "speed": 0.2
+    },
+    {
+      "x": 0.58,
+      "y": 0.0,
+      "z": 0.5,
+      "a": 3.14,
+      "b": 0.3665,
+      "c": 2.8791,
+      "speed": 0.3,
+      "planner": "lin"
+    }
+  ]
+}
@@ -1,4 +1,16 @@
 #!/usr/bin/env python3
+"""
+Запуск заданной последовательности движений манипулятора iiwa.
+
+Каждая точка в конфиге может быть либо суставной (type: joints),
+либо декартовой (type: pose). Тип определяется автоматически по наличию
+ключа "joints" или координат "x/y/z".
+
+Запуск:
+  ros2 run iiwa_planning motion_sequence_runner \
+    --ros-args -p config_path:=/path/to/config.json -p n_iterations:=2
+"""
+
 import json
 import shutil
 import threading
@@ -18,37 +30,45 @@ from rosidl_runtime_py.utilities import get_message
 from iiwa_msgs.action import MoveToJoints, MoveToPose


-class IiwaTestRunner(Node):
+def _is_joints_waypoint(wp: dict) -> bool:
+    return "joints" in wp
+
+
+class MotionSequenceRunner(Node):
    def __init__(self):
-        super().__init__('iiwa_test_runner')
+        super().__init__('motion_sequence_runner')

        self.declare_parameter('n_iterations', 3)
        self.declare_parameter('bag_path', '')
        self.declare_parameter('config_path', '')
        self.declare_parameter('topics', [''])
-        self.declare_parameter("delay_between_iterations", 5.0)
+        self.declare_parameter('delay_between_iterations', 5.0)
+        self.declare_parameter('joints_action', 'cobot/move_to_joints')
+        self.declare_parameter('pose_action', 'cobot/move_to_pose')

        self._n_iter = self.get_parameter('n_iterations').value
-        self._delay_between_iterations = self.get_parameter("delay_between_iterations").value
+        self._delay = self.get_parameter('delay_between_iterations').value
        self._bag_path = self.get_parameter('bag_path').value
        topics_param = self.get_parameter('topics').value
-        # [''] means not specified — record all topics
        self._topics_param: list[str] = [t for t in topics_param if t]
        config_path = self.get_parameter('config_path').value
+        joints_action = self.get_parameter('joints_action').value
+        pose_action = self.get_parameter('pose_action').value

        cfg = self._load_config(config_path)
-        self._home_joints: list[float] = cfg['home_joints']
-        self._poses: list[dict] = cfg['poses']
+        self._home: dict = cfg['home']
+        self._waypoints: list[dict] = cfg['waypoints']

        self._cb_group = ReentrantCallbackGroup()
        self._joints_client = ActionClient(
-            self, MoveToJoints, '/iiwa/move_to_joints',
+            self, MoveToJoints, joints_action,
            callback_group=self._cb_group,
        )
        self._pose_client = ActionClient(
-            self, MoveToPose, '/iiwa/move_to_pose',
+            self, MoveToPose, pose_action,
            callback_group=self._cb_group,
        )
+        self.get_logger().info(f'joints_action={joints_action}  pose_action={pose_action}')

        self._writer: rosbag2_py.SequentialWriter | None = None
        self._registered_topics: set[str] = set()
@@ -60,14 +80,19 @@ class IiwaTestRunner(Node):
        else:
            self.get_logger().info('bag_path not set — recording disabled')

-    # Config
+    # ── Config ──────────────────────────────────────────────────────────────
+
    def _load_config(self, config_path: str) -> dict:
-        path = Path(config_path) if config_path else Path(__file__).parent / 'motion_config.json'
+        path = (
+            Path(config_path) if config_path
+            else Path(__file__).parent / 'motion_sequence_config.json'
+        )
        self.get_logger().info(f'Loading config from {path}')
        with open(path) as f:
            return json.load(f)

-    # Bag files
+    # ── Bag recording ────────────────────────────────────────────────────────
+
    def _init_bag(self):
        bag_dir = Path(self._bag_path)
        if bag_dir.exists():
@@ -90,7 +115,9 @@ class IiwaTestRunner(Node):

        topics = self._topics_param if self._topics_param else list(available.keys())
        if not self._topics_param:
-            self.get_logger().info(f'topics not set — recording all {len(topics)} available topics')
+            self.get_logger().info(
+                f'topics not set — recording all {len(topics)} available topics'
+            )

        for topic in topics:
            if topic not in available:
@@ -132,26 +159,26 @@ class IiwaTestRunner(Node):
        if self._writer:
            del self._writer
            self._writer = None
-            self.get_logger().info(f'[BAG] closed → {self._bag_path}')
+            self.get_logger().info(f'Bag closed → {self._bag_path}')

-    # Action helpers
-    def _send_joints_goal(self, joints: list[float], speed: float = 0.1) -> bool:
+    # ── Action helpers ────────────────────────────────────────────────────────
+
+    def _send_joints_goal(self, wp: dict) -> bool:
        goal = MoveToJoints.Goal()
-        goal.joints = joints
-        goal.speed = speed
+        goal.joints = wp['joints']
+        goal.speed = float(wp.get('speed', 0.1))

-        self.get_logger().info(f'[MOVE] joints → {joints}')
+        self.get_logger().info(f'[JOINTS] → {goal.joints}')
        if not self._joints_client.wait_for_server(timeout_sec=10.0):
            self.get_logger().error('MoveToJoints server unavailable')
            return False

        done = threading.Event()
-        success_holder: list[bool] = [False]
+        result_holder: list[bool] = [False]

        def _on_result(future):
-            res = future.result().result
-            success_holder[0] = res.success
-            self.get_logger().info(f'[MOVE] joints done: success={res.success}')
+            result_holder[0] = future.result().result.success
+            self.get_logger().info(f'[JOINTS] done: success={result_holder[0]}')
            done.set()

        def _on_goal(future):
@@ -164,30 +191,29 @@ class IiwaTestRunner(Node):

        self._joints_client.send_goal_async(goal).add_done_callback(_on_goal)
        done.wait()
-        return success_holder[0]
+        return result_holder[0]

-    def _send_pose_goal(self, *, x, y, z, a, b, c,
-                        speed: float = 0.1, 
-                        planner: str = 'lin',
-                        id: int = None) -> bool:
+    def _send_pose_goal(self, wp: dict, idx: int | None = None) -> bool:
        goal = MoveToPose.Goal()
-        goal.x, goal.y, goal.z = x, y, z
-        goal.a, goal.b, goal.c = a, b, c
-        goal.speed = speed
-        goal.planner = planner
+        goal.x, goal.y, goal.z = wp['x'], wp['y'], wp['z']
+        goal.a, goal.b, goal.c = wp['a'], wp['b'], wp['c']
+        goal.speed = float(wp.get('speed', 0.1))
+        goal.planner = wp.get('planner', 'lin')

-        self.get_logger().info(f'[MOVE] {id if id is not None else ""} pose → x={x} y={y} z={z} planner={planner}')
+        label = f'#{idx} ' if idx is not None else ''
+        self.get_logger().info(
+            f'[POSE] {label}→ x={goal.x} y={goal.y} z={goal.z} planner={goal.planner}'
+        )
        if not self._pose_client.wait_for_server(timeout_sec=10.0):
            self.get_logger().error('MoveToPose server unavailable')
            return False

        done = threading.Event()
-        success_holder: list[bool] = [False]
+        result_holder: list[bool] = [False]

        def _on_result(future):
-            res = future.result().result
-            success_holder[0] = res.success
-            self.get_logger().info(f'[MOVE] pose done: success={res.success}')
+            result_holder[0] = future.result().result.success
+            self.get_logger().info(f'[POSE] done: success={result_holder[0]}')
            done.set()

        def _on_goal(future):
@@ -200,21 +226,27 @@ class IiwaTestRunner(Node):

        self._pose_client.send_goal_async(goal).add_done_callback(_on_goal)
        done.wait()
-        return success_holder[0]
+        return result_holder[0]
+
+    def _send_waypoint(self, wp: dict, idx: int | None = None) -> bool:
+        if _is_joints_waypoint(wp):
+            return self._send_joints_goal(wp)
+        return self._send_pose_goal(wp, idx=idx)
+
+    # ── Main sequence ─────────────────────────────────────────────────────────

-    # Main sequence
    def run(self, done_event: threading.Event):
        try:
            for i in range(self._n_iter):
                self.get_logger().info(f'======= Iteration {i + 1}/{self._n_iter} =======')

-                self._send_joints_goal(self._home_joints)
+                self._send_waypoint(self._home)

-                for i, pose in enumerate(self._poses):
-                    self._send_pose_goal(id=i, **pose)
+                for idx, wp in enumerate(self._waypoints):
+                    self._send_waypoint(wp, idx=idx)

-                self._send_joints_goal(self._home_joints)
-                time.sleep(self._delay_between_iterations)
+                self._send_waypoint(self._home)
+                time.sleep(self._delay)

            self.get_logger().info('======= Sequence complete =======')
        finally:
@@ -224,7 +256,7 @@ class IiwaTestRunner(Node):

 def main():
    rclpy.init()
-    node = IiwaTestRunner()
+    node = MotionSequenceRunner()

    executor = MultiThreadedExecutor()
    executor.add_node(node)
@@ -1,913 +0,0 @@
-{
-  "home_joints": [
-    0.0,
-    0.0,
-    0.0,
-    -1.57,
-    0.0,
-    1.57,
-    0.0
-  ],
-  "poses": [
-    {
-      "x": 0.6,
-      "y": 0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 2.7916,
-      "speed": 0.4,
-      "planner": "ptp"
-    },
-    {
-      "x": 0.6,
-      "y": -0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 2.7916,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.58,
-      "y": -0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 2.8791,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.58,
-      "y": 0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 2.8791,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.56,
-      "y": 0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 2.9666,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.56,
-      "y": -0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 2.9666,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.54,
-      "y": -0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 3.0541,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.54,
-      "y": 0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 3.0541,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.52,
-      "y": 0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 3.1416,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.52,
-      "y": -0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 3.1416,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.5,
-      "y": -0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 3.2291,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.5,
-      "y": 0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 3.2291,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.48,
-      "y": 0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 3.3166,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.48,
-      "y": -0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 3.3166,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.46,
-      "y": -0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 3.4041,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.46,
-      "y": 0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 3.4041,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.44,
-      "y": 0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 3.4916,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.44,
-      "y": -0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 3.4916,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.44,
-      "y": -0.1,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 3.4916,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.44,
-      "y": 0.1,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 3.4916,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.46,
-      "y": 0.1,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 3.4041,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.46,
-      "y": -0.1,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 3.4041,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.48,
-      "y": -0.1,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 3.3166,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.48,
-      "y": 0.1,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 3.3166,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.5,
-      "y": 0.1,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 3.2291,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.5,
-      "y": -0.1,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 3.2291,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.52,
-      "y": -0.1,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 3.1416,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.52,
-      "y": 0.1,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 3.1416,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.54,
-      "y": 0.1,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 3.0541,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.54,
-      "y": -0.1,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 3.0541,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.56,
-      "y": -0.1,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 2.9666,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.56,
-      "y": 0.1,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 2.9666,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.58,
-      "y": 0.1,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 2.8791,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.58,
-      "y": -0.1,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 2.8791,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.6,
-      "y": -0.1,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 2.7916,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.6,
-      "y": 0.1,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 2.7916,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.6,
-      "y": 0.1,
-      "z": 0.45,
-      "a": 3.14,
-      "b": 0.4189,
-      "c": 2.7916,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.6,
-      "y": -0.1,
-      "z": 0.45,
-      "a": 3.14,
-      "b": 0.4189,
-      "c": 2.7916,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.58,
-      "y": -0.1,
-      "z": 0.45,
-      "a": 3.14,
-      "b": 0.4189,
-      "c": 2.8791,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.58,
-      "y": 0.1,
-      "z": 0.45,
-      "a": 3.14,
-      "b": 0.4189,
-      "c": 2.8791,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.56,
-      "y": 0.1,
-      "z": 0.45,
-      "a": 3.14,
-      "b": 0.4189,
-      "c": 2.9666,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.56,
-      "y": -0.1,
-      "z": 0.45,
-      "a": 3.14,
-      "b": 0.4189,
-      "c": 2.9666,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.54,
-      "y": -0.1,
-      "z": 0.45,
-      "a": 3.14,
-      "b": 0.4189,
-      "c": 3.0541,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.54,
-      "y": 0.1,
-      "z": 0.45,
-      "a": 3.14,
-      "b": 0.4189,
-      "c": 3.0541,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.52,
-      "y": 0.1,
-      "z": 0.45,
-      "a": 3.14,
-      "b": 0.4189,
-      "c": 3.1416,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.52,
-      "y": -0.1,
-      "z": 0.45,
-      "a": 3.14,
-      "b": 0.4189,
-      "c": 3.1416,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.5,
-      "y": -0.1,
-      "z": 0.45,
-      "a": 3.14,
-      "b": 0.4189,
-      "c": 3.2291,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.5,
-      "y": 0.1,
-      "z": 0.45,
-      "a": 3.14,
-      "b": 0.4189,
-      "c": 3.2291,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.48,
-      "y": 0.1,
-      "z": 0.45,
-      "a": 3.14,
-      "b": 0.4189,
-      "c": 3.3166,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.48,
-      "y": -0.1,
-      "z": 0.45,
-      "a": 3.14,
-      "b": 0.4189,
-      "c": 3.3166,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.46,
-      "y": -0.1,
-      "z": 0.45,
-      "a": 3.14,
-      "b": 0.4189,
-      "c": 3.4041,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.46,
-      "y": 0.1,
-      "z": 0.45,
-      "a": 3.14,
-      "b": 0.4189,
-      "c": 3.4041,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.44,
-      "y": 0.1,
-      "z": 0.45,
-      "a": 3.14,
-      "b": 0.4189,
-      "c": 3.4916,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.44,
-      "y": -0.1,
-      "z": 0.45,
-      "a": 3.14,
-      "b": 0.4189,
-      "c": 3.4916,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.44,
-      "y": -0.1,
-      "z": 0.4,
-      "a": 3.14,
-      "b": 0.4712,
-      "c": 3.4916,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.44,
-      "y": 0.1,
-      "z": 0.4,
-      "a": 3.14,
-      "b": 0.4712,
-      "c": 3.4916,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.46,
-      "y": 0.1,
-      "z": 0.4,
-      "a": 3.14,
-      "b": 0.4712,
-      "c": 3.4041,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.46,
-      "y": -0.1,
-      "z": 0.4,
-      "a": 3.14,
-      "b": 0.4712,
-      "c": 3.4041,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.48,
-      "y": -0.1,
-      "z": 0.4,
-      "a": 3.14,
-      "b": 0.4712,
-      "c": 3.3166,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.48,
-      "y": 0.1,
-      "z": 0.4,
-      "a": 3.14,
-      "b": 0.4712,
-      "c": 3.3166,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.5,
-      "y": 0.1,
-      "z": 0.4,
-      "a": 3.14,
-      "b": 0.4712,
-      "c": 3.2291,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.5,
-      "y": -0.1,
-      "z": 0.4,
-      "a": 3.14,
-      "b": 0.4712,
-      "c": 3.2291,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.52,
-      "y": -0.1,
-      "z": 0.4,
-      "a": 3.14,
-      "b": 0.4712,
-      "c": 3.1416,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.52,
-      "y": 0.1,
-      "z": 0.4,
-      "a": 3.14,
-      "b": 0.4712,
-      "c": 3.1416,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.54,
-      "y": 0.1,
-      "z": 0.4,
-      "a": 3.14,
-      "b": 0.4712,
-      "c": 3.0541,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.54,
-      "y": -0.1,
-      "z": 0.4,
-      "a": 3.14,
-      "b": 0.4712,
-      "c": 3.0541,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.56,
-      "y": -0.1,
-      "z": 0.4,
-      "a": 3.14,
-      "b": 0.4712,
-      "c": 2.9666,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.56,
-      "y": 0.1,
-      "z": 0.4,
-      "a": 3.14,
-      "b": 0.4712,
-      "c": 2.9666,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.58,
-      "y": 0.1,
-      "z": 0.4,
-      "a": 3.14,
-      "b": 0.4712,
-      "c": 2.8791,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.58,
-      "y": -0.1,
-      "z": 0.4,
-      "a": 3.14,
-      "b": 0.4712,
-      "c": 2.8791,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.6,
-      "y": -0.1,
-      "z": 0.4,
-      "a": 3.14,
-      "b": 0.4712,
-      "c": 2.7916,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.6,
-      "y": 0.1,
-      "z": 0.4,
-      "a": 3.14,
-      "b": 0.4712,
-      "c": 2.7916,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.6,
-      "y": 0.1,
-      "z": 0.35,
-      "a": 3.14,
-      "b": 0.5236,
-      "c": 2.7916,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.6,
-      "y": -0.1,
-      "z": 0.35,
-      "a": 3.14,
-      "b": 0.5236,
-      "c": 2.7916,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.58,
-      "y": -0.1,
-      "z": 0.35,
-      "a": 3.14,
-      "b": 0.5236,
-      "c": 2.8791,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.58,
-      "y": 0.1,
-      "z": 0.35,
-      "a": 3.14,
-      "b": 0.5236,
-      "c": 2.8791,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.56,
-      "y": 0.1,
-      "z": 0.35,
-      "a": 3.14,
-      "b": 0.5236,
-      "c": 2.9666,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.56,
-      "y": -0.1,
-      "z": 0.35,
-      "a": 3.14,
-      "b": 0.5236,
-      "c": 2.9666,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.54,
-      "y": -0.1,
-      "z": 0.35,
-      "a": 3.14,
-      "b": 0.5236,
-      "c": 3.0541,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.54,
-      "y": 0.1,
-      "z": 0.35,
-      "a": 3.14,
-      "b": 0.5236,
-      "c": 3.0541,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.52,
-      "y": 0.1,
-      "z": 0.35,
-      "a": 3.14,
-      "b": 0.5236,
-      "c": 3.1416,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.52,
-      "y": -0.1,
-      "z": 0.35,
-      "a": 3.14,
-      "b": 0.5236,
-      "c": 3.1416,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.5,
-      "y": -0.1,
-      "z": 0.35,
-      "a": 3.14,
-      "b": 0.5236,
-      "c": 3.2291,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.5,
-      "y": 0.1,
-      "z": 0.35,
-      "a": 3.14,
-      "b": 0.5236,
-      "c": 3.2291,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.48,
-      "y": 0.1,
-      "z": 0.35,
-      "a": 3.14,
-      "b": 0.5236,
-      "c": 3.3166,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.48,
-      "y": -0.1,
-      "z": 0.35,
-      "a": 3.14,
-      "b": 0.5236,
-      "c": 3.3166,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.46,
-      "y": -0.1,
-      "z": 0.35,
-      "a": 3.14,
-      "b": 0.5236,
-      "c": 3.4041,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.46,
-      "y": 0.1,
-      "z": 0.35,
-      "a": 3.14,
-      "b": 0.5236,
-      "c": 3.4041,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "x": 0.44,
-      "y": 0.1,
-      "z": 0.35,
-      "a": 3.14,
-      "b": 0.5236,
-      "c": 3.4916,
-      "speed": 0.4,
-      "planner": "lin"
-    },
-    {
-      "x": 0.44,
-      "y": -0.1,
-      "z": 0.35,
-      "a": 3.14,
-      "b": 0.5236,
-      "c": 3.4916,
-      "speed": 0.3,
-      "planner": "lin"
-    }
-  ]
-}
@@ -10,7 +10,6 @@ setup(
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
-        ('share/' + package_name, ['iiwa_utils/motion_config.json']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
@@ -22,7 +21,6 @@ setup(
        'console_scripts': [
            "object_spawner = iiwa_utils.object_spawner:main",
            "camera_spawner = iiwa_utils.camera_spawner:main",
-            "test_motion_sequence = iiwa_utils.test_motion_sequence:main",
        ],
    },
 )