Run as `python3 manipulation_test.py`

manipulation_test.py

#!/usr/bin/env python3
import rclpy
from action_msgs.msg import GoalStatus
from rclpy.action import ActionClient
from control_msgs.action import FollowJointTrajectory
from rclpy.node import Node
from trajectory_msgs.msg import JointTrajectoryPoint

import sys
import time


class ManipulationTest(Node):

    def __init__(self):
        super().__init__('manipulation_test')
        self.count = 0
        self.trajectory_client = ActionClient(self, FollowJointTrajectory, '/stretch_controller/follow_joint_trajectory')
        if not self.trajectory_client.wait_for_server(timeout_sec=10.0):
            self.get_logger().error('Unable to connect to arm action server. Timeout exceeded.')
            sys.exit()

    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info('Goal rejected :(')
            return

        self.get_logger().info('Goal accepted :)')
        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    def get_result_callback(self, future):
        status = future.result().status
        if status == GoalStatus.STATUS_SUCCEEDED:
            self.get_logger().info('Goal succeeded!')
        else:
            self.get_logger().info('Goal failed with status: {0}'.format(status))

        # aperture range: [-0.1285204486235414, 0.34289112948906764]
        time.sleep(1)
        self.count += 1
        if self.count % 2 == 0:
            self.move_to_configuration(0.091)
        else:
            self.move_to_configuration(-0.11)

        
    def move_to_configuration(self, gripper_aperture):
        trajectory_goal = FollowJointTrajectory.Goal()
        goal_time_tolerance = rclpy.duration.Duration(seconds=1.0)
        trajectory_goal.goal_time_tolerance = goal_time_tolerance.to_msg()
        trajectory_goal.trajectory.joint_names = ['gripper_aperture']
        
        point = JointTrajectoryPoint()
        point.time_from_start = rclpy.duration.Duration(seconds=2.0).to_msg()

        point.positions = [gripper_aperture]

        trajectory_goal.trajectory.joint_names = trajectory_goal.trajectory.joint_names
        trajectory_goal.trajectory.points = [point]
        trajectory_goal.trajectory.header.stamp = self.get_clock().now().to_msg()

        self.get_logger().info(f'Sending goal #{self.count} to value: {gripper_aperture}...')
        self._send_goal_future = self.trajectory_client.send_goal_async(trajectory_goal)
        self._send_goal_future.add_done_callback(self.goal_response_callback)


if __name__ == '__main__':
    rclpy.init(args=None)
    manipulation_test = ManipulationTest()
    manipulation_test.move_to_configuration(0.091)
    rclpy.spin(manipulation_test)