AS: connection with Pd

workspace/src/joint_angles.py

@@ -1,12 +1,13 @@
 from osc4py3.as_eventloop import *
 from osc4py3 import oscbuildparse
+import time
 
 def main():
     # Start the OSC system
     osc_startup()
 
     # Make client channels to send packets
-    osc_udp_client("127.0.0.1", 8000, "osc_client")
+    osc_udp_client("172.18.0.3", 8000, "osc_client")
 
     
     # Example joint positions to send
@@ -17,12 +18,33 @@ def main():
     joint_positions5 = [-0.5,-0.6, 0.2,0.0, 0.0, 0.0]
 
     msg = oscbuildparse.OSCMessage("/joint_trajectory", None, [joint_positions1, joint_positions2, joint_positions3, joint_positions4, joint_positions5])
-    print("Sending joint positions")
-    # Send the OSC message
     osc_send(msg, "osc_client")
-    print("Sent joint positions")
     osc_process()
+    print("Sending joint positions")
+    '''
+    time.sleep(2)
+    msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions2)
+    osc_send(msg, "osc_client")
+    osc_process()
+    print("Sent joint positions2")
+    time.sleep(3)
+    msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions3)
+    osc_send(msg, "osc_client")
+    osc_process()
+    print("Sent joint positions3")
+    time.sleep(3)
+    msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions4)
+    osc_send(msg, "osc_client")
+    osc_process()
+    print("Sent joint positions4")
+    time.sleep(3)
+    msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions5)
+    osc_send(msg, "osc_client")
+    osc_process()
+    print("Sent joint positions5")
+    time.sleep(3)
+    '''
     osc_terminate()
 
 if __name__ == "__main__":
-    main()
+    main()

workspace/src/joint_control/joint_control/cart_tcp_server.py

@@ -33,7 +33,7 @@ class ScaledJointTrajectoryPublisher(Node):
 
     def tcp_coordinates_handler(self, *args):
         """Handles incoming OSC messages for tcp position."""
-        #time1 = time.time()
+        time1 = time.time()
         if len(args) == len(self.joint_positions):
             x, y, z, roll, pitch, yaw = args
             duration = 4.0  # Default duration
@@ -62,10 +62,10 @@ class ScaledJointTrajectoryPublisher(Node):
         if sol[1]:
             joint_positions = list(sol[0])
             self.send_trajectory(joint_positions, duration)
-            print(f"Computed joint positions: {joint_positions}")
+            #print(f"Computed joint positions: {joint_positions}")
         else:
             print("Inverse kinematics failed")
-        #print(f"Frequency: {1/(time.time() - time1)} Hz")
+        print(f"Frequency: {1/(time.time() - time1)} Hz")
 
     def send_trajectory(self, joint_positions, duration=4.0):
         """Publish a joint trajectory command to move the robot."""

workspace/src/joint_control/joint_control/joint_angles_server.py

@@ -29,8 +29,10 @@ class ScaledJointTrajectoryPublisher(Node):
 
     def joint_angles_handler(self, *args):
         """Handles incoming OSC messages for joint positions."""
+        print(args)
         if len(args) == len(self.joint_positions): 
-            self.joint_positions = args
+            self.joint_positions = list(args)
+            print(self.joint_positions)
             self.send_trajectory(self.joint_positions)
         elif len(args) == len(self.joint_positions) + 1:
             self.joint_positions = args[:-1]
@@ -39,11 +41,12 @@ class ScaledJointTrajectoryPublisher(Node):
 
 
 
-    def send_trajectory(self, joint_positions, duration=3.0):
+    def send_trajectory(self, joint_positions, duration=0.01):
         """Publish a joint trajectory command to move the robot."""
         msg = JointTrajectory()
         msg.joint_names = self.joint_names
         point = JointTrajectoryPoint()
+        joint_positions = [float(joint) for joint in joint_positions]
         point.positions = joint_positions  # Updated joint positions
         point.time_from_start.sec = int(duration)
         point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
@@ -55,8 +58,8 @@ class ScaledJointTrajectoryPublisher(Node):
 
 def main():
     """Main function to get joint names and start the ROS 2 & OSC system."""
-
-    tree = ET.parse('/BA/robot.urdf')
+    robot_urdf = input("Enter the path to the URDF file: ")
+    tree = ET.parse(robot_urdf)
     root = tree.getroot()
     joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
 

workspace/src/joint_control/joint_control/trajectory_server_cart.py

@@ -15,20 +15,26 @@ class ScaledJointTrajectoryPublisher(Node):
         super().__init__('scaled_joint_trajectory_publisher')
         
         self.robot = robot
+        self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
+
+        if self.trajectroy_topic_name == "":
+            self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
 
         # ROS2 Publisher
         self.publisher = self.create_publisher(
             JointTrajectory, 
-            '/scaled_joint_trajectory_controller/joint_trajectory', 
+            self.trajectroy_topic_name, 
             10
         )
 
         # Store received joint positions
         self.joint_names = joint_names
+        
+        self.port = 8000 # UDP port
 
         osc_startup()
-        osc_udp_server("0.0.0.0", 8000, "osc_server")
-        print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
+        osc_udp_server("0.0.0.0", self.port, "osc_server")
+        print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
         # Register OSC handler
         osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
 
@@ -41,26 +47,53 @@ class ScaledJointTrajectoryPublisher(Node):
         joint_positions = [0.0] * len(self.joint_names)
         steps = 30
         vel = 0.4
-        if len(args[0]) == len(self.joint_names):
+        if True: #len(args[0]) == len(self.joint_names):
             n=2.0
             for i in range(len(args)-1):
+                print(f'i = {i}')
                 x, y, z, roll, pitch, yaw = args[i]
+                print(1)
                 Tep1 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
+                print(2)
                 x, y, z, roll, pitch, yaw = args[i+1]
+                print(3)
                 Tep2 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
+                print(4)
                 cart_traj = rtb.ctraj(Tep1, Tep2, steps)
-                for j in range(steps-1):
+                print(cart_traj)
+                print(5)
+                for j in range(steps):
+                    print(f'j = {j}')
+                    print(6)
                     sol = self.robot.ik_LM(cart_traj[j], q0=joint_positions)
-                    dist = np.linalg.norm(cart_traj[j].t - cart_traj[j+1].t)
-                    point = JointTrajectoryPoint()
-                    point.positions = list(sol[0])
-                    joint_positions = list(sol[0])
-                    point.time_from_start.sec = int(n)
-                    point.time_from_start.nanosec = int((n - int(n)) * 1e9)
-                    n+=dist/vel
-                    n+=0.1
-                    msg.points.append(point)
+                    print(7)
+                    if sol[1] == 1:
+                        print(8)
+                        if j == 0: dist = vel*n
+                        else: dist = np.linalg.norm(cart_traj[j].t - cart_traj[j-1].t)
+                        print(9)
+                        point = JointTrajectoryPoint()
+                        print(10)
+                        point.positions = list(sol[0])
+                        print(11)
+                        joint_positions = list(sol[0])
+                        print(12)
+                        point.time_from_start.sec = int(n)
+                        print(13)
+                        point.time_from_start.nanosec = int((n - int(n)) * 1e9)
+                        print(14)
+                        n+=dist/vel
+                        print(16)
+                        msg.points.append(point)
+                        print(17)
+                    else: print('IK could not find a solution!')
+        print(18)
+        self.publisher.publish(msg)
+        print(19)
+        print(f"published joint positions {msg.points[-1]}")
+        print(f'Frequency: {round(1/(time.time()-time1),2)} Hz')
 
+'''
         elif len(args[0]) == len(self.joint_names) + 1:
             for i in range(len(args)):
                 x, y, z, roll, pitch, yaw, timetag = args[i]
@@ -72,20 +105,18 @@ class ScaledJointTrajectoryPublisher(Node):
                     sol = self.robot.ik_LM(Tep, q0=joint_positions)
 
         else:
-            print("Invalid number or format of arguments")
-        
-        self.publisher.publish(msg)
-        print("published joint positions")
-        print(f'Frequency: {round(1/(time.time()-time1),2)} Hz')
+            print("Invalid number or format of arguments")'''
+    
 
 
 def main():
     """Main function to get joint names and start the ROS 2 & OSC system."""
-
-    tree = ET.parse('/BA/robot.urdf')
+    path_to_urdf = input("Enter the path to the URDF file: ")
+    tree = ET.parse(path_to_urdf)
     root = tree.getroot()
     joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
-    robot = rtb.ERobot.URDF('/BA/robot.urdf')
+    robot = rtb.ERobot.URDF(path_to_urdf)
+    print(robot)
     rclpy.init()
 
     node = ScaledJointTrajectoryPublisher(joint_names, robot)

workspace/src/joint_control/setup.py

@@ -25,6 +25,10 @@ setup(
             'cart_coords = joint_control.cart_tcp_server:main',
             'trajectory_server = joint_control.trajectory_server:main',
             'trajectory_server_cart = joint_control.trajectory_server_cart:main',
+            'plugdata = joint_control.plugdata:main',
+            'plugdata2 = joint_control.plugdata2:main',
+            'test=joint_control.test:main',
+            'plugdata3 = joint_control.plugdata3:main',
         ],
     },
 )