All components for Inverse Control implemented and tested + publishing the current endeffector state

franka_iml_experiment/dmp_node.py

@@ -9,6 +9,7 @@ from active_bo_msgs.msg import DMP
 import pydmps
 import pydmps.dmp_discrete
 
+
 import numpy as np
 
 

franka_iml_experiment/move_square.py

@@ -0,0 +1,49 @@
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseArray, Pose, Point, Quaternion
+from std_msgs.msg import Empty
+
+
+class MoveFrankaSquareNode(Node):
+
+    def __init__(self):
+        super().__init__('move_franka_square')
+        self.publisher_ = self.create_publisher(PoseArray, 'moveit_interface/task_space_trajectory', 10)
+        self.subscriber_ = self.create_subscription(Empty, 'moveit_interface/trigger', self.trigger_callback, 10)
+
+    def trigger_callback(self, msg):
+        # Create a PoseArray message
+        pose_array = PoseArray()
+
+        # Define the four poses in the square
+        z_offset = 0.10  # 10 cm above the base joint
+        side_length = 0.5
+
+        poses = [
+            Pose(position=Point(x=0.4, y=0.3, z=0.15), orientation=Quaternion(x=1., y=0., z=0., w=0.)),
+            Pose(position=Point(x=0.4, y=-0.3, z=0.15), orientation=Quaternion(x=1., y=0., z=0., w=0.)),
+            # Pose(position=Point(x=-0.1, y=0.1, z=z_offset), de=Quaternion(x=0., y=1., z=0., w=0.)),
+            # Pose(position=Point(x=-0.1, y=-0.1, z=z_offset), orientation=Quaternion(x=0., y=1., z=0., w=0.))
+        ]
+
+        pose_array.poses = poses
+
+        # Publish the PoseArray
+        self.publisher_.publish(pose_array)
+        self.get_logger().info('Published PoseArray to moveit_interface/task_space_trajectory')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    move_franka_square_node = MoveFrankaSquareNode()
+
+    rclpy.spin(move_franka_square_node)
+
+    move_franka_square_node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

setup.py

@@ -25,6 +25,7 @@ setup(
         'console_scripts': [
             'dmp_node = franka_iml_experiment.dmp_node:main',
             'dmp_test = franka_iml_experiment.dmp_test:main',
+            'move_square = franka_iml_experiment.move_square:main',
         ],
     },
 )