AS: code update

osc_recieve.py

@@ -6,7 +6,7 @@ def handler(address,*args):
 
 osc_startup()
 osc_udp_server("0.0.0.0", 7000, "osc_server")
-osc_method("/joint_states/counter", handler, argscheme=osm.OSCARG_ADDRESS+ osm.OSCARG_DATAUNPACK)
+osc_method("/time", handler, argscheme=osm.OSCARG_ADDRESS+ osm.OSCARG_DATAUNPACK)
 
 
 while True:

test/collision.py

@@ -1,29 +0,0 @@
-import pybullet as p
-
-# Start PyBullet simulation
-physicsClient = p.connect(p.DIRECT)  # or p.GUI for graphical interface
-
-# Load your robot URDF
-robot_id = p.loadURDF("/ur10e.urdf", useFixedBase=True)
-
-# Set joint angles
-joint_angles = [0, 0, 3.141/2, 0, 0, 0]
-for i, angle in enumerate(joint_angles):
-    p.setJointMotorControl2(robot_id, i, p.POSITION_CONTROL, targetPosition=angle)
-
-# Step the simulation to update the robot's state
-p.stepSimulation()
-
-# Check if any link is colliding
-collision = False
-for i in range(p.getNumBodies()):
-    contact_points = p.getContactPoints(robot_id)
-    print(contact_points)
-    if len(contact_points) > 0:
-        collision = True
-        break
-
-if collision:
-    print("The robot has collided with itself.")
-else:
-    print("No collision detected.")

test/commands_recording.py

@@ -0,0 +1,26 @@
+from pythonosc.dispatcher import Dispatcher
+from pythonosc import osc_server
+import csv
+import time
+
+# CSV file setup
+csv_file = open("osc_log.csv", mode="w", newline="")
+csv_writer = csv.writer(csv_file)
+csv_writer.writerow(["timestamp", "x", "y", "z", "roll", "pitch", "yaw"])
+
+# Handler for OSC messages
+def handle_pose(address, *args):
+    timestamp = time.time()
+    csv_writer.writerow([timestamp] + list(args))
+    csv_file.flush()  # Optional: ensures data is written immediately
+
+# Setup dispatcher and bind address
+dispatcher = Dispatcher()
+dispatcher.map("/tcp_coordinates", handle_pose)  # Accept messages sent to address /pose
+
+# Start server
+ip = "0.0.0.0"
+port = 8000
+server = osc_server.ThreadingOSCUDPServer((ip, port), dispatcher)
+print(f"Listening for OSC messages on {ip}:{port}...")
+server.serve_forever()

test/get_ip.py

@@ -0,0 +1,14 @@
+import socket
+
+def get_ip_address():
+    try:
+        # Connect to an external address — no data is actually sent
+        s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        s.connect(("8.8.8.8", 80))  # Google's DNS
+        ip = s.getsockname()[0]
+        s.close()
+        return ip
+    except Exception:
+        return "127.0.0.1"  # fallback to localhost
+
+print("Local IP address:", get_ip_address())

test/ros2_ws/src/joint_states_control/move_scaled_joint.py

@@ -12,28 +12,31 @@ class ScaledJointTrajectoryPublisher(Node):
             10
         )
 
-    def send_trajectory(self, joint_positions, duration=2.0):
+        try:
-        """Publish a joint trajectory command to move the UR10e."""
+            msg = JointTrajectory()
-        msg = JointTrajectory()
+            msg.joint_names = [
-        msg.joint_names = [
+                "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
-            "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
+                "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
-            "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
+            ]
-        ]
 
-        point = JointTrajectoryPoint()
+            point = JointTrajectoryPoint()
-        point.positions = joint_positions  # Target joint positions
+            point.positions = [2.0]*6  # Target joint positions
-        point.time_from_start.sec = int(duration)  # Duration in seconds
+            point.velocities = [2.0] * 6
+            point.accelerations = [2.0] * 6
+            #point.time_from_start.sec = int(2)
+            #point.time_from_start.nanosec = int(0)
+            msg.points.append(point)
 
-        msg.points.append(point)
+            self.publisher.publish(msg)
-
+        except Exception as e:
-        self.publisher.publish(msg)
+            print(f"Error publishing joint trajectory: {e}")
 
 def main():
     rclpy.init()
     node = ScaledJointTrajectoryPublisher()
 
     # Move to a target position
-    node.send_trajectory([0.0]*6)
+
 
     rclpy.spin(node)
     node.destroy_node()

workspace/src/osc_ros2/osc_ros2/osc_ros2.py

@@ -13,6 +13,7 @@ from osc4py3 import oscbuildparse
 import time
 import os
 import re
+import socket
 
 class JointNameListener(Node):
     def __init__(self):
@@ -35,11 +36,25 @@ class OSC_ROS2_interface(Node):
     def __init__(self, joint_names, joint_velocity_limits, robot, cost_mask):
         super().__init__('scaled_joint_trajectory_publisher')
         
+        while True:
+            try:
+                self.trajectory_topic_name = input("Enter the topic name to which the joint trajectory should be sent (press Enter for default: '/scaled_joint_trajectory_controller/joint_trajectory'): ").strip()
+                if self.trajectory_topic_name == "":
+                    self.trajectory_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
+                    break
+                elif self.trajectory_topic_name.startswith("/"):
+                    break
+                else:
+                    print("Invalid topic name. A valid topic name should start with '/'.")
+            except Exception as e:
+                print(f"An error occurred: {e}")
+
+
 
         # ROS2 Publisher
         self.publisher = self.create_publisher(
             JointTrajectory, 
-            '/scaled_joint_trajectory_controller/joint_trajectory', 
+            self.trajectory_topic_name, 
             1
         )
 
@@ -73,6 +88,7 @@ class OSC_ROS2_interface(Node):
             50: "FATAL",
         }
         self.speed_scaling = 0.2
+        self.new = False
 
 
         while True:
@@ -182,9 +198,29 @@ class OSC_ROS2_interface(Node):
                                     continue
                                     
                                 if lower_limit:
-                                    lim[0][i] = float(lower_limit)
+                                    if lower_limit<lim[0][i]:
+                                        while True:
+                                            sure = input(f"Are you sure you want to set the lower limit to {lower_limit} rad which is less than the default limit {lim[0][i]}(y/n): ").strip().lower()
+                                            if sure == 'y':
+                                                lim[0][i] = float(lower_limit)
+                                                break
+                                            elif sure == 'n':
+                                                print("Lower limit not changed.")
+                                                break
+                                            print("Invalid input. Please enter 'y' or 'n'.")
+                                    else: lim[0][i] = float(lower_limit)
                                 if upper_limit:
-                                    lim[1][i] = float(upper_limit)
+                                    if upper_limit<lim[0][i]:
+                                        while True:
+                                            sure = input(f"Are you sure you want to set the upper limit to {upper_limit} rad which is more than the default limit {lim[1][i]}(y/n): ").strip().lower()
+                                            if sure == 'y':
+                                                lim[1][i] = float(upper_limit)
+                                                break
+                                            elif sure == 'n':
+                                                print("Upper limit not changed.")
+                                                break
+                                            print("Invalid input. Please enter 'y' or 'n'.")
+                                    else: lim[0][i] = float(lower_limit)
                                 self.robot.qlim = lim
                                 print(f"New limits for joint '{self.joint_names[i]}': [{self.robot.qlim[0][i]} {self.robot.qlim[1][i]}] rad")
                                 print("-" * 50)
@@ -245,10 +281,11 @@ class OSC_ROS2_interface(Node):
 
         # Register OSC handler
         osc_method("/joint_positions", self.joint_positions_handler, argscheme=osm.OSCARG_DATAUNPACK)
-        #osc_method("/joint_position/*", self.joint_position_handler, argscheme=osm.OSCARG_DATAUNPACK)
+        osc_method("/joint_position/*", self.joint_position_handler, argscheme=osm.OSCARG_DATAUNPACK)
         osc_method("/tcp_coordinates", self.tcp_coordinates_handler, argscheme=osm.OSCARG_DATAUNPACK)
         osc_method("/joint_trajectory", self.joint_trajectory_handler, argscheme=osm.OSCARG_DATAUNPACK)
         osc_method("/cartesian_trajectory", self.cartesian_trajectory_handler, argscheme=osm.OSCARG_DATAUNPACK)
+        osc_method("/speed_scaling", self.speed_scaling_handler, argscheme=osm.OSCARG_DATAUNPACK)
         print('--' * 50)
         self.hz = float(input("Enter the desired refresh frequency (Hz): "))
         print()
@@ -262,7 +299,7 @@ class OSC_ROS2_interface(Node):
         print()
         print('=-=' * 50)
         print()
-        print(f'Ready to receive OSC messages on 0.0.0.0:{commands_port}')
+        print(f'Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{commands_port}')
         print()
         print('=-=' * 50)
         print()
@@ -272,9 +309,26 @@ class OSC_ROS2_interface(Node):
         self.get_logger().info(f'Sending log messages to {log_ip}:{log_port}')
         self.create_timer(1/self.hz, self.update_position)  # Timer to update the position
 
+    def speed_scaling_handler(self, *args):
+        """Handles incoming OSC messages for speed scaling."""
+        try:
+            if len(args) == 1:
+                if args[0] < 0:
+                    self.speed_scaling = -float(args[0])
+                else:
+                    self.speed_scaling = float(args[0])
+                self.get_logger().info(f"Speed scaling set to {self.speed_scaling}")
+            else:
+                self.get_logger().warn(f"Invalid number of arguments for speed scaling. Expected 1, but got {len(args)}.")
+        except Exception as e:
+            self.get_logger().fatal(f"speed_scaling_handler: {e}")
+    
     def joint_trajectory_handler(self, *args):
         pass
 
+    def joint_position_handler(self, *args):
+        pass
+
     def cartesian_trajectory_handler(self, *args):
         """Handles incoming OSC messages for cartesian trajectory."""
         if self.robot:
@@ -285,30 +339,34 @@ class OSC_ROS2_interface(Node):
 
     def joint_positions_handler(self, *args):
         """Handles incoming OSC messages for joint positions."""
+        try:
+            if len(args) == len(self.joint_names):
+                desired_joint_positions = [float(i) for i in list(args)] + [None]
+            elif len(args) == len(self.joint_names) + 1:
+                desired_joint_positions = [float(i) for i in list(args)]
+            else:
+                self.get_logger().warn(f"joint_positions_handler: Invalid number of arguments for joint positions. Expected {len(self.joint_names)} ([q0, q1, q2, ... q{len(self.joint_names)}]) or {len(self.joint_names)+1} ([q0, q1, q2, ... q{len(self.joint_names)}, duration]), but got {len(args)}.")
+                return
 
-        if len(args) == len(self.joint_names):
+            # Check if joint positions exceed limits
-            desired_joint_positions = [float(i) for i in list(args)] + [None]
+            for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]):  # Exclude duration if present
-        elif len(args) == len(self.joint_names) + 1:
+                if position < self.robot.qlim[0][i]:
-            desired_joint_positions = [float(i) for i in list(args)]
+                    desired_joint_positions[i] = self.robot.qlim[0][i]
-        else:
+                    self.get_logger().warn(
-            self.get_logger().warn(f"joint_positions_handler: Invalid number of arguments for joint positions. Expected {len(self.joint_names)} ([q0, q1, q2, ... q{len(self.joint_names)}]) or {len(self.joint_names)+1} ([q0, q1, q2, ... q{len(self.joint_names)}, duration]), but got {len(args)}.")
+                        f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[0][i]}."
+                    )
+                elif position > self.robot.qlim[1][i]:
+                    desired_joint_positions[i] = self.robot.qlim[1][i]
+                    self.get_logger().warn(
+                        f"joint_positions_handler:Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[1][i]}."
+                    )
+
+            self.desired = ["joint_positions"] + desired_joint_positions
+            self.new = True
+        except Exception as e:
+            self.get_logger().fatal(f"joint_positions_handler: {e}")
             return
 
-        # Check if joint positions exceed limits
-        for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]):  # Exclude duration if present
-            if position < self.robot.qlim[0][i]:
-                desired_joint_positions[i] = self.robot.qlim[0][i]
-                self.get_logger().warn(
-                    f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[0][i]}."
-                )
-            elif position > self.robot.qlim[1][i]:
-                desired_joint_positions[i] = self.robot.qlim[1][i]
-                self.get_logger().warn(
-                    f"joint_positions_handler:Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[1][i]}."
-                )
-
-        self.desired = ["joint_positions"] + desired_joint_positions
-
     def tcp_coordinates_handler(self, *args):
     # Ensure the desired joint positions are within the specified limits
         if self.robot:
@@ -342,6 +400,7 @@ class OSC_ROS2_interface(Node):
                     )
 
                 self.desired = ["tcp_coordinates", x, y, z, r, p, yaw, duration]
+                self.new = True
             except Exception as e:
                 self.get_logger().fatal(f"tcp_coordinates_handler: {e}")
         else:
@@ -352,6 +411,8 @@ class OSC_ROS2_interface(Node):
     def joint_states_callback(self, msg: JointState):
         """Callback function to handle incoming joint states."""
         try:
+            msg_time = oscbuildparse.OSCMessage(f"/time", ',s', [str(time.time())])
+            osc_send(msg_time, "osc_client")
             if not(self.joint_names): self.joint_names = msg.name
             joint_position_dict = dict(zip(msg.name, msg.position))
             self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
@@ -554,23 +615,23 @@ class OSC_ROS2_interface(Node):
     def update_position(self):
         """Calls the appropriate function to update the robot's position."""
         try:
-            if self.desired is None:
+            if self.desired is None or not(self.new):
-                return
-            if self.desired == self.previous_desired:
-                msg_log = oscbuildparse.OSCMessage(f"/log/INFO", ',isss', [20, str(time.time()) , 'interface - update_position', "Desired joint positions are the same as previous."])
-                osc_send(msg_log, "osc_client")
                 return
             
             if self.desired[0] == "joint_positions":
+                self.new = False
                 self.send_joint_positions()
                 return
             elif self.desired[0] == "tcp_coordinates":
+                self.new = False
                 self.send_tcp_coordinates()
                 return
             elif self.desired[0] == "joint_trajectory":
+                self.new = False
                 self.send_joint_trajectory()
                 return
             elif self.desired[0] == "cartesian_trajectory":
+                self.new = False
                 self.send_cartesian_trajectory()
                 return
         except Exception as e:
@@ -622,7 +683,7 @@ def main():
                     print("Invalid path. Please enter a valid 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']
+            joint_names = [link.name for link in robot.links if link.isjoint]
             robot = rtb.ERobot.URDF(robot_urdf)
             print(robot)
             joint_velocity_limits = {}