diff --git a/osc_recieve.py b/osc_recieve.py index 5eaa694..2decd4f 100644 --- a/osc_recieve.py +++ b/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: diff --git a/test/collision.py b/test/collision.py deleted file mode 100644 index 9c2a59c..0000000 --- a/test/collision.py +++ /dev/null @@ -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.") diff --git a/test/commands_recording.py b/test/commands_recording.py new file mode 100644 index 0000000..5992b43 --- /dev/null +++ b/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() diff --git a/test/get_ip.py b/test/get_ip.py new file mode 100644 index 0000000..9328ec3 --- /dev/null +++ b/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()) diff --git a/test/ros2_ws/src/joint_states_control/move_scaled_joint.py b/test/ros2_ws/src/joint_states_control/move_scaled_joint.py index 4f7e203..35d72d1 100644 --- a/test/ros2_ws/src/joint_states_control/move_scaled_joint.py +++ b/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): - """Publish a joint trajectory command to move the UR10e.""" - msg = JointTrajectory() - msg.joint_names = [ - "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", - "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" - ] + try: + msg = JointTrajectory() + msg.joint_names = [ + "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", + "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" + ] - point = JointTrajectoryPoint() - point.positions = joint_positions # Target joint positions - point.time_from_start.sec = int(duration) # Duration in seconds + point = JointTrajectoryPoint() + point.positions = [2.0]*6 # Target joint positions + 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) + self.publisher.publish(msg) + except Exception as e: + 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() diff --git a/workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc b/workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc index 2a0b252..85c71a9 100644 Binary files a/workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc and b/workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc differ diff --git a/workspace/src/osc_ros2/osc_ros2/osc_ros2.py b/workspace/src/osc_ros2/osc_ros2/osc_ros2.py index 410d961..b96e39c 100644 --- a/workspace/src/osc_ros2/osc_ros2/osc_ros2.py +++ b/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 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: - 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") + if self.desired is None or not(self.new): 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 = {}