AS: code update

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):
-        """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()