From b67b2a51748babaadef0751ecb667c6d674b701f Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Wed, 7 May 2025 23:12:12 +0200 Subject: [PATCH] AS: code update --- osc_recieve.py | 2 +- test/collision.py | 29 ---- test/commands_recording.py | 26 ++++ test/get_ip.py | 14 ++ .../joint_states_control/move_scaled_joint.py | 31 +++-- .../__pycache__/osc_ros2.cpython-310.pyc | Bin 23111 -> 23280 bytes workspace/src/osc_ros2/osc_ros2/osc_ros2.py | 125 +++++++++++++----- 7 files changed, 151 insertions(+), 76 deletions(-) delete mode 100644 test/collision.py create mode 100644 test/commands_recording.py create mode 100644 test/get_ip.py 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 2a0b2522ef75ded9bc191e6cde17678d520589a0..85c71a9bab19a133946b3daa35b4017c53625ba4 100644 GIT binary patch delta 4317 zcmb_feQXrR72nz2Tkn0^zB_+^V;g_sZy*BWK)}X?*p6cxgCRYgJ%{nxzB7AE2-aSX z=qQ1ZP?JfNsww0yQJW@hLX&GMRh62kZA8@mku)u|y(&fhkW#g&MEVg`i3)vh_AoX{ z`&VzZZ{NK6y_xsk%zHC;={w}w>%9+riuA)Ja_@go%nkCC#1{wS_>%tJgmz#KS%je?!XH73myt zrw+-rB-;yp)F}GR$*F*k`>z9Go$RlFCTY3m*msl%!nSt!$<7nBd8&p z;enBI43elZJW0@65)XeITOKl>?lj0V0_{I9eU3?zRv%ITy9*})#SehLJHx-JR~RHt z48d2cuZ;`fT%D0+sC0K+;=ILB=sldTrvrwS4syPM4nbIAje1kCncn+2LL2BXh}Nx= z+y|1DRgw{qw62hNsfT-Bt>@FxxM@h6?&XGf=@{?3+1g`dfs5mY`iyX1#vbthbAfve z3kNDUf&T@!Pj63jz}uM=k{G>l!}`B83?b2#6cX#{1jmWVVX$FYxXLb1LYSOsi;?KaLa!y)mpKUh@!|ItUO4{+11Jjd$(C8-qd(iE^%mpNy|t5 zT1Y#|KC1Cp=0Q-v9$Pb5s{rSOcPG4&_lO`Y{!|xmUjY?@@P))HVEAQ@+8t2dA**<01S^8AJjqbb2GDi;ZcR3yyG+7DTE z&8{Io&6y^KSOBy6)Nz4IBZ4gG)? z-sFywqe6q5#cEEIYG$ns+8+gvv_d57*+(6vN-a32ok=BX-K2A|C>Wjz>Ar<}BtR!vieQ+wUfGTOrLBH;wYf!X2?03sR5P^^2eJY8&^8-DsHnOxF&x>ulpi@>r^;DUT=_Pxdued9*K~ zaZ5nJ;fx1QHP?$8;~QiaI=b2A<_j?_t#){MX}vBA!V+2Z=|X}~LPcHFC0w33nI=go zD(bQ!QYj8u0n2TP6dNR{z`|PeEM4V130mV~oSYUO5e^b6Q_Bezk+^XHXIp`@o46PU zDz^b0pybqkfHdI(YDlvbhYE5vOul4#@c6TxR96TrRt*YvVFEm)+F0gDh7U zf^Gu}Hx_ckYS;{WY&t6`Mja4XhZ$I|>Y-km2i|(T0_FtGhxkC&2CW!KFvQ7G1-GFJ zGbnoo<>IKK+fcxN&r^6V#=}%OvwzE11Qt}9K&h2jO=OoHpBC92Vjx7 zzvqA$&5gSGxaEeMyWm9gjoeHmO$RCrcWk-SzT9crUAaQhyWEGynr4_rNUM!lB8MxW z%BdkE@`CsSt%pw1N1D#Np)&J+MR>Uqe6DkjGZO*Q-1Kpi{X=FC~NwCXeS zfkm-7qTK~~!gjY8v5RdJ${A=ZcJu7DZ^&aC+gsKpK(wMhB(eG2vW%CDP0*Y-6ZeVs zud(iqm&n)I2OUl11?K7O;QU%8hJY8C*4dd695)5;b<&<94KG4z+tA4Nfj}hUpAKlF zD3H<0v>J{~`67W3HS1d$@1ZkOdRkhXSwh*Rey(>|Pl{jKYK9YZv)fZRFm*WDnaU}XduSu7+r!d_c{N;!epFS2tx z%h>%Jij`B)T&%;OrIt3d%Ie2(J}pU$``M*U?~p6(>CI)NjlI5km@K8Tx_1#W$%eOd zHN!=$1h!9UoKO|6T|`@ejarr{PYQgaXR1YcNt4l%^WkHI5M|Yl&LuTMKU;j81~}z6L-vCYlUlUJy()+tpJ| z{=weZ+$%|1K6|~Vg4D6$?mYJAoV;7VD=SaMa?M@;$qYW z7st2tLQ@kCXvMJcFp1g+kPh#&!&{3;D?78bNXmrAvm747j!tpZJ6k(r@2?88r1kwj zLS+UP4*j+z>DVHqV{RrLaS_@nr1s5pWDjo}BTuD%xvi2o&cjAY%g6jj+qJKxr0w4$ zZ5WH6_qN_V7q|8n*7CzYtt{a6AfgQb zGOaw|pF0wWq-_rb=E75OUkI@A-bzuMWG8!jieJXTzKl4@KN-@U6b zZ)Lc|Gd!Jz$vqdI&Rh+Sfa3a;ZTIg8DP|wuQ%SzToCED-h}}7Gow!)fU@>`-?HjD< z{}gBVCz6kmV6D-ra2hNb+=vEq22%jbzOp#-s<9i8Q|!&b(JgbRGmiu>RN5>O3lgj+ zrZbn3yUimsR;yyeLrv9Hz(&>pQ3aVeL?NagRwY$c%UtzpqfJt4)J68}P#t+Pb!q4n ziET3kS^^i`8+u_}fCJb{p}fGqB_UB{I4~<{S=4Dbg2j<*h768^SX1<+IAwZLl+SzxBBIAL!OH`yUt_<&>$yFR?1 zRHr8HdxhZrcC?lltaY@utP!HNsJ67W@(V2wa@sOCGe4sbYOkaYk5&@rqZk7$#|;E3 z4qkBq|J=M^KcrnjNwyZA4C^yf5j`B9n+!jw zZ9voTm^1ft3pPuTgi&Tgf(xzT#9BFWg{-RI)2DXNhv|{IKo7`;NEZ-QaPahVfENzp Y28s{1%qK_8$rfVfqYkPE!0#LX0llh3X#fBK delta 4181 zcmb_fYiu0V6`nIQJDz>o-d(@F_WJF$9mkJ6C?p|q;s*|~L!89KB-w6q*Tx%rbMCq4-gD;eCqIK%Uj)}tr_(0j-@^MP_kVisysHx4Ki`_}>KBNW*iH+? z78aAzSS60_#DQZ6ANz=txK0aWRpDxqe?-)#!c`dsz5sAjQF;$5=?&>T6lP}R2FNc$ z?^b*yd}8%#=4s0bOX8yLC7Y7~f?l7hPYZFkAw(SWV(eXWY`9AWvgL?8FKT~AWDi3| zWfP;l%HNW<7d2a%#wsAUjeGNwUh~Z`S;<=dE+&0RNR{DS9>bGJm8S(#bwr$(G>Gmo zJbb*Gk9S&y2_X(SF8i~N2H2ZVw&e1ztV^%qF(y`KI zkHsXh-GGY%xqCsn8BD2}#!4QItdQ(!s{52bShDwH$ z66Y<3LUwXKK<+WDWEbaK$Oz`6Rd0)|BD>EaGNHGSJt%6wDMHC!l&oGM8AVA)j>Jnm zJQsC(F}XKs2GXf}c_3c0kL#93V@4h>@jgSnC@jb;H2abXT-mbOe#63n%GUsMaR&@5 zBE(xjff1Ty+pA#B#?V&TS{LI58 zk4m(oq7~L<_E(I6Mb;;$!}Ln!Tl8sPClt|le3ujplXcQqRY0j@mG{fcQ&sE1t%3?} zd_++HtIF(6->V)67wz%KEYk=q%e?F_1jh`ri}Hz(7S+B@UkP|DQ500tk86e!7UZ1x z^y3q|0D_`WpRBKXF3P)vY`rv+VP@@+yAh%^BuLR^e)T8SlciGcW4Vp)ktWB~V0 z!u^zqvg8?0NuDmyU)QdqpVh8~3$(57uA-p9`hpV4BXXHwNM*tiS-VEhG7Tx107ERn zxPmH?a03fxyXfkgTH041hGzPFeFPq%TUr|JEw~8nEG}nugud5MMjvhQBc_0!Z@69> zuxbZUqj?yqWQ3(go1=4$K5M|DvG%5YhhA%J?bS*d&q*2MCXv*m>sTW)ZmArK!3#s8 z3{{dBITe~g`O1qtLzzo=MDp=8Vh?6L*{~Y6 ziM+HJx9bIlofIUojd06C;wBzkW}#QOC?pm{;$?>)6D+etqB94F73IPZRh&gBS(KBb zN^U`wW>M}e3WLHNmLaNqX-f1}kww9_ggIqC!p!*d4Ts@0Todvwk$cz)VAXtJ(|$2t z5HIA8D~&?dlQ9<QCU*i(2!Wc}uC1Y|sY9W| zNBs2ujsQ(}#1vls8+i4t>fE~OhgcFhMU!H~^s~Js^juMS9y%HWDKdR~4IUH!;XN0$dCy)zx%uY~_ zJaw*JsJq0}yhrin@RbarY#bws^&umR^v$(%N|NDD)8M-G${A#5n|Nwo=(l8ET-PG2 z?DzbQ1E(^L8}0(wMQ1jy zap~gUW}VtLOkZ@If5M| zSc3<_F@n84Rg1bheCpGzh#G>87)Mj8*??R$o!e9g|DZSS=&h491rcJZ(8y!8pN2T= zPHrNWp~oW<-Z@L?%!VQwxU&@6k!xeu&^~%;!zxMC4$?pH_XG4x{(g+MZ)`<*ns#ii z3H}5f$x$$rxKj_9?y;DJNK2|UEhHr@p2Gc7+!fD{7wBzAvoVtVHeKm3S(b4Mo((hEDj1RFg+R1R0^^F!4GpD~voGx7-|tmSAv=9hH`K5Ia_ z*)B9EQ{8f>*^bDA)HA$$<218jw?cl;(b$bbV^>3sbrUoCw-|TJ5?HXBk3K!zTIWYL zR*OUxWN?T=LOr2Ms;ZW|n$>2Tq}Hp)>8Ha@@K~mN=Nu$983L)m+v0!p(xiaLUPzT0 z4tB6eW!z~vBIU6U4JYy8{qNnV7r81d5T{;|swCA6bra99ahXx=ZQW|{A18(*(smzC7e(OvgJZ6>qp3P3eIw7UUL&^e@y z^vdoEcrNqWZa?64?O%JEmB&R~^I2N9w;$$dXzwXnKY9Ueem;5|9HO6$Chd(Z(+{)E zG|`9d-Jp!4LF`F-?cV$9Z6JdJHoPO+@LvJt3sq53#{TgV58^AX9@@LF#@&lP7S&*I zIy4y#1_OSrnLfC04k8rBR#&$(r6qcNdgi2d9p_oJ$7VwMWN2zSq~EXoi4Kny)pxNV t*!RXfg#1!m!ellkWBa7_(sN_2&_!PwtML|y1^Gh4{8tCndoa+i{s#l=BPIX< 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 = {}