remove gitignore

test/collision.py

@@ -0,0 +1,29 @@
+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/joint_states_recording.py

@@ -0,0 +1,78 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+import csv
+import os
+import roboticstoolbox as rtb
+
+class JointStateLogger(Node):
+    def __init__(self):
+        super().__init__('joint_state_logger_split')
+
+        # Subscribe to /joint_states topic
+        self.subscription = self.create_subscription(
+            JointState,
+            '/joint_states',
+            self.joint_states_callback,
+            1000
+        )
+        self.robot = rtb.ERobot.URDF('/BA/ur10e.urdf')
+        # Directory to store logs
+        self.log_dir = '/BA/joint_states_logs'
+        os.makedirs(self.log_dir, exist_ok=True)
+
+        self.log_dir_cart = '/BA/joint_states_cart_logs'
+        os.makedirs(self.log_dir_cart, exist_ok=True)
+
+    def joint_states_callback(self, msg: JointState):
+        # Use ROS time as timestamp
+        stamp = msg.header.stamp
+        timestamp = f"{stamp.sec}.{str(stamp.nanosec).zfill(9)}"
+
+        for i, name in enumerate(msg.name):
+            # File per joint
+            file_path = os.path.join(self.log_dir, f"{name}.csv")
+
+            # If file doesn't exist, create with header
+            if not os.path.exists(file_path):
+                with open(file_path, mode='w', newline='') as f:
+                    writer = csv.writer(f)
+                    writer.writerow(["timestamp", "position", "velocity", "effort"])
+
+            # Get values if they exist
+            position = msg.position[i] if i < len(msg.position) else ''
+            velocity = msg.velocity[i] if i < len(msg.velocity) else ''
+            effort = msg.effort[i] if i < len(msg.effort) else ''
+
+            # Append to joint-specific CSV
+            with open(file_path, mode='a', newline='') as f:
+                writer = csv.writer(f)
+                writer.writerow([timestamp, position, velocity, effort])
+        
+        file_path_cart = os.path.join(self.log_dir_cart, "cartesian.csv")
+        if not os.path.exists(file_path_cart):
+            with open(file_path_cart, mode='w', newline='') as f:
+                writer = csv.writer(f)
+                writer.writerow(["timestamp", "x", "y", "z", "roll", "pitch", "yaw"])
+        with open(file_path_cart, mode='a', newline='') as f:
+            writer = csv.writer(f)
+            [x,y,z] = self.robot.fkine(list([msg.position[-1]])+list(msg.position[:4])).t
+            [roll, pitch, yaw] = self.robot.fkine(list([msg.position[-1]])+list(msg.position[:4])).rpy()
+            writer.writerow([timestamp, x, y, z, roll, pitch, yaw])
+            
+
+        
+
+def main():
+    rclpy.init()
+    node = JointStateLogger()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        print("")
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

test/joint_trajectory_recording.py

@@ -0,0 +1,81 @@
+import rclpy
+from rclpy.node import Node
+from trajectory_msgs.msg import JointTrajectory
+import csv
+import os
+import roboticstoolbox as rtb
+
+class JointTrajectoryLogger(Node):
+    def __init__(self):
+        super().__init__('joint_trajectory_logger_minimal')
+
+        # Folder to store CSV files
+        self.log_dir = '/BA/joint_trajectories'
+        os.makedirs(self.log_dir, exist_ok=True)
+
+        self.log_dir_cart = '/BA/joint_trajectories_cart'
+        os.makedirs(self.log_dir_cart, exist_ok=True)
+
+        # Subscriber to JointTrajectory messages
+        self.subscription = self.create_subscription(
+            JointTrajectory,
+            '/scaled_joint_trajectory_controller/joint_trajectory',
+            self.trajectory_callback,
+            10
+        )
+
+        self.counter = 0
+        self.robot = rtb.ERobot.URDF('/BA/ur10e.urdf')
+
+    def trajectory_callback(self, msg: JointTrajectory):
+        self.counter += 1
+
+        # Create a unique filename
+        filename = f"trajectory_{self.counter:04d}.csv"
+        file_path = os.path.join(self.log_dir, filename)
+        # Header: timestamp + joint names
+        header = ['timestamp'] + msg.joint_names
+        recive_time = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
+        with open(file_path, mode='w', newline='') as file:
+            writer = csv.writer(file)
+            writer.writerow(header)
+
+            for point in msg.points:
+                timestamp = recive_time + point.time_from_start.sec + point.time_from_start.nanosec * 1e-9
+                row = [timestamp] + list(point.positions)
+                writer.writerow(row)
+
+        filename_cart = f"trajectory_{self.counter:04d}_cart.csv"
+        file_path_cart = os.path.join(self.log_dir_cart, filename_cart)
+        header = ['timestamp', 'x', 'y', 'z', 'roll', 'pitch', 'yaw']
+        # Convert joint angles to cartesian coordinates
+        joint_angles = point.positions
+        
+        # Write to CSV
+        with open(file_path_cart, mode='w', newline='') as file:
+            writer = csv.writer(file)
+            writer.writerow(header)
+
+            for point in msg.points:
+                timestamp = recive_time + point.time_from_start.sec + point.time_from_start.nanosec * 1e-9
+                T = self.robot.fkine(point.positions)
+                x = T.t[0]
+                y = T.t[1]
+                z = T.t[2]
+                roll, pitch, yaw = T.rpy()
+                row = [timestamp, x, y, z, roll, pitch, yaw]
+                writer.writerow(row)
+
+def main():
+    rclpy.init()
+    node = JointTrajectoryLogger()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        print("")
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

test/load_robot.py

@@ -0,0 +1,46 @@
+import xml.etree.ElementTree as ET
+import roboticstoolbox as rtb
+
+def main():
+    """Main function to get joint names and start the ROS 2 & OSC system."""
+    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']
+    robot = rtb.ERobot.URDF(robot_urdf)
+    joint_velocity_limits = {}
+
+    # Iterate over all joints in the URDF
+    for joint in root.findall('.//joint'):
+        joint_name = joint.get('name')  # Get the name of the joint
+        
+        # Look for the <limit> tag under each joint
+        limit = joint.find('limit')
+        
+        if limit is not None:
+            # Extract the velocity limit (if it exists)
+            velocity_limit = limit.get('velocity')
+            
+            if velocity_limit is not None:
+                joint_velocity_limits[joint_name] = float(velocity_limit)
+    
+    while True:
+        try:
+            print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].")
+            print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotaion.")
+            cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
+            if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
+                break
+            else:
+                print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
+        except ValueError:
+            print("Invalid input. Please enter integers only.")
+    print(f"Cost mask: {cost_mask}")
+
+    print(f'robot: {robot}')
+    print(f'joint names: {joint_names}')
+    print(f'joint velocity limits: {joint_velocity_limits}')
+
+
+if __name__ == '__main__':
+    main()

test/log_recording.py

@@ -0,0 +1,61 @@
+import rclpy
+from rclpy.node import Node
+from rcl_interfaces.msg import Log
+import csv
+import os
+
+class RosoutLogger(Node):
+    def __init__(self):
+        super().__init__('rosout_logger_csv')
+
+        # Log file path
+        self.log_dir = '/BA/rosout_logs'
+        os.makedirs(self.log_dir, exist_ok=True)
+        self.csv_file_path = os.path.join(self.log_dir, 'rosout_log.csv')
+
+        # Initialize CSV with header if it doesn't exist
+        if not os.path.exists(self.csv_file_path):
+            with open(self.csv_file_path, mode='w', newline='') as f:
+                writer = csv.writer(f)
+                writer.writerow(['timestamp', 'level', 'logger_name', 'message'])
+
+        # Subscribe to /rosout
+        self.subscription = self.create_subscription(
+            Log,
+            '/rosout',
+            self.rosout_callback,
+            10
+        )
+
+    def rosout_callback(self, msg: Log):
+        # Format timestamp
+        timestamp = f"{msg.stamp.sec}.{str(msg.stamp.nanosec).zfill(9)}"
+
+        # Convert level integer to text
+        level_dict = {
+            Log.DEBUG: 'DEBUG',
+            Log.INFO: 'INFO',
+            Log.WARN: 'WARN',
+            Log.ERROR: 'ERROR',
+            Log.FATAL: 'FATAL'
+        }
+        level_text = level_dict.get(msg.level, f"LEVEL_{msg.level}")
+
+        # Write row to CSV
+        with open(self.csv_file_path, mode='a', newline='') as f:
+            writer = csv.writer(f)
+            writer.writerow([timestamp, level_text, msg.name, msg.msg])
+
+def main():
+    rclpy.init()
+    node = RosoutLogger()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        node.get_logger().info("Interrupted by user.")
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

test/timetag_test_pub.py

@@ -0,0 +1,28 @@
+# Import needed modules from osc4py3
+from osc4py3.as_eventloop import *
+from osc4py3 import oscbuildparse
+import time
+
+# Start the system.
+osc_startup()
+
+# Make client channels to send packets.
+osc_udp_client("172.18.0.3", 8000, "aclientname")
+
+# Buils a complete bundle, and postpone its executions by 10 sec.
+exectime = time.time() + 10   # execute in 10 seconds
+stamp = oscbuildparse.unixtime2timetag(exectime)
+
+# Build a simple message and send it.
+msg = oscbuildparse.OSCMessage("/test/me", ",sift", ["text", 672, 8.871, stamp])
+osc_send(msg, "aclientname")
+
+# Build a message with autodetection of data types, and send it.
+msg = oscbuildparse.OSCMessage("/test/me", None, ["text", 672, 8.871])
+osc_send(msg, "aclientname")
+    # …
+osc_process()
+    # …
+
+# Properly close the system.
+osc_terminate()

test/timetag_test_sub.py

@@ -0,0 +1,19 @@
+# Import needed modules from osc4py3
+from osc4py3.as_allthreads import *
+from osc4py3 import oscmethod as osm
+from osc4py3 import oscbuildparse
+
+def handlerfunction(*args):
+    # Will receive message data unpacked in s, x, y
+    print(args[-1].sec)
+    print(oscbuildparse.timetag2unixtime(args[-1]))
+# Start the system.
+osc_startup()
+
+# Make server channels to receive packets.
+osc_udp_server("0.0.0.0", 8000, "anotherserver")
+
+# Associate Python functions with message address patterns, using default
+# argument scheme OSCARG_DATAUNPACK.
+osc_method("/test/me", handlerfunction, argscheme=osm.OSCARG_TYPETAGS + osm.OSCARG_DATAUNPACK)
+