nwegy/OSC_ROS2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AS

Browse Source
This commit is contained in:
Alexander Schaefer
2025-05-12 20:20:00 +02:00
parent 4da5338ca8
commit 472cbc6b08
53 changed files with 2629 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
workspace/src/joint_angles.py
View File

@@ -12,15 +12,17 @@ def main():
#osc_udp_client("192.168.1.24", 8000, "osc_client")
osc_udp_client("127.0.0.1", 8000, "osc_client")
# Example joint positions to send
joint_positions1 = [0.4,0.3, 0.5, 0.0, 0.0, 0.0]#, 6.0]
joint_positions2 = [0.4,-0.4, 0.6, 0.0,0.0, 0.0]
joint_positions3 = [-0.4,-0.4, 0.6, 0.0, 0.0, 0.0]#, 6.0]
joint_positions4 = [-0.4,0.4, 0.6, 0.0, 0.0, 0.0]#, 6.0]
joint_positions5 = [0.4,0.4, 0.6, 0.0, 0.0, 0.0]#, 6.0]
joint_positions1 = [0.4,0.4, 1.0, 0.0, 0.0, 0.0, 3]
joint_positions2 = [0.4,-0.4, 0.2, 0.0,0.0, 0.0]
joint_positions3 = [0.4,-0.4, 0.6, 0.0, 0.0, 0.0]#, 6.0]
joint_positions4 = [0.4,0.4, 0.6, 0.0, 0.0, 0.0]#, 6.0]
joint_positions5 = [0.4,0.4, 0.2, 0.0, 0.0, 0.0]#, 6.0]
msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions1)
osc_send(msg, "osc_client")
osc_process()
print(time.time())
print(joint_positions1)
print("Sending joint positions")
'''
time.sleep(2)

BIN
workspace/src/joint_control/joint_control/__pycache__/__init__.cpython-310.pyc
View File

Binary file not shown.

BIN
workspace/src/joint_control/joint_control/__pycache__/trajectory_server_new.cpython-310.pyc
View File

Binary file not shown.

2
workspace/src/joint_control/joint_control/trajectory_server_new.py
View File

@@ -81,7 +81,7 @@ class ScaledJointTrajectoryPublisher(Node):
dt = 0.01
tacc = 0.5
print(f'length viapoints: {len(viapoints)}')
traj = rtb.mstraj(np.array(viapoints), q0 = self.current_joint_positions ,dt=dt, tacc=tacc, qdmax=[1 * i for i in self.joint_velocity_limits])
traj = rtb.mstraj(np.array(viapoints), q0 = self.current_joint_positions ,dt=dt, tacc=tacc, qdmax=[0.2 * i for i in self.joint_velocity_limits])
print(len(traj.q))
print(len(traj.t))
print(traj.t)

BIN
workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc
View File

Binary file not shown.

144
workspace/src/osc_ros2/osc_ros2/osc_ros2.py
View File

@@ -14,6 +14,7 @@ import time
import os
import re
import socket
import csv
class JointNameListener(Node):
def __init__(self):
@@ -91,6 +92,9 @@ class OSC_ROS2_interface(Node):
self.new = False
self.latency = []
while True:
try:
print('+-' * 50)
@@ -308,6 +312,9 @@ class OSC_ROS2_interface(Node):
self.get_logger().info(f'Sending joint states to {state_ip}:{state_port}')
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
self.create_timer(3, self.reset_prev) # reset the previous desired position
def reset_prev(self): self.previous_desired = None
def speed_scaling_handler(self, *args):
"""Handles incoming OSC messages for speed scaling."""
@@ -369,7 +376,6 @@ class OSC_ROS2_interface(Node):
def tcp_coordinates_handler(self, *args):
# Ensure the desired joint positions are within the specified limits
print("tcp_coordinates_handler")
if self.robot:
try:
if len(args) == 6:
@@ -453,41 +459,22 @@ class OSC_ROS2_interface(Node):
def send_joint_positions(self):
pass
self.previous_desired = None
def trapezoidal_timestamps(self, n_points, total_duration, accel_fraction=0.2):
"""
Generate timestamps for a trapezoidal velocity profile.
def trapezoidal_timestamps(self, num_points,total_duration, flat_ratio = 0.3):
if num_points == 2:
return [0, total_duration]
n = int(num_points*(1-flat_ratio)/2)
start = np.cos(np.linspace(0, np.pi, n))+2
end = np.cos(np.linspace(-np.pi, 0, n))+2
flat = np.ones(num_points-2*n)
Parameters:
n_points (int): Total number of trajectory points.
total_duration (float): Duration of the full motion [seconds].
accel_fraction (float): Fraction of the total time spent accelerating and decelerating.
Default is 0.2 (i.e., 20% accel, 60% cruise, 20% decel)
Returns:
list of float: Timestamps for each point [seconds].
"""
if n_points == 2:
return [0.0, total_duration]
# Time fractions
ta = accel_fraction * total_duration # acceleration time
tc = total_duration - 2 * ta # constant velocity time
# Number of points per segment
n_accel = int(n_points * accel_fraction)
if n_accel == 0:
return list(np.linspace(0, total_duration, n_points))
n_cruise = n_points - 2*n_accel
# Time vectors for each segment
t_accel = np.linspace(0, ta, n_accel, endpoint=False)
t_cruise = np.linspace(ta, ta + tc, n_cruise, endpoint=False)
t_decel = np.linspace(ta + tc, total_duration, n_accel, endpoint=True)
timestamps = np.concatenate([t_accel, t_cruise, t_decel])
return list(timestamps)
timestamps = np.concatenate((start, flat, end))
timestamps *= total_duration / timestamps.sum()
timestamps = np.cumsum(timestamps)
return timestamps.tolist()
def send_tcp_coordinates(self):
@@ -496,18 +483,64 @@ class OSC_ROS2_interface(Node):
msg = JointTrajectory()
msg.joint_names = self.joint_names
steps_per_m = 100
[x,y,z] = self.robot.fkine(self.current_joint_positions).t
[roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
if self.previous_desired == None:
[x,y,z] = self.robot.fkine(self.current_joint_positions).t
[roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
else:
[x,y,z] = self.previous_desired[1:4]
[roll, pitch, yaw] = self.previous_desired[4:-1]
x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7]
#self.prev_pose = self.desired[1:]
self.previous_desired = self.desired
steps = int(np.linalg.norm(np.array([x1, y1, z1])- self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
if steps < 2: steps = 2
cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)]
'''if self.previous_desired:
[x,y,z] = self.previous_desired[1:4]
q0 = sm.UnitQuaternion.RPY(self.previous_desired[3], self.previous_desired[4], self.previous_desired[5])
else:
[x, y, z] = self.robot.fkine(self.current_joint_positions).t
q0 = sm.UnitQuaternion(self.robot.fkine(self.current_joint_positions).R)
x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7]
q1 = sm.UnitQuaternion.RPY(roll1, pitch1, yaw1)
steps = int(np.linalg.norm(np.array([x1, y1, z1]) - self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
if steps < 2:
steps = 2
cart_traj = []
for i in range(steps):
alpha = i / (steps - 1)
# Convert to arrays for robust interpolation if needed
q0_array = q0.vec
q1_array = q1.vec
dot = np.dot(q0_array, q1_array)
dot = np.clip(dot, -1.0, 1.0)
if abs(dot) > 0.9995:
# Linear interpolation + normalization
q_interp_array = (1 - alpha) * q0_array + alpha * q1_array
q_interp_array = q_interp_array / np.linalg.norm(q_interp_array)
q_interp = sm.UnitQuaternion(q_interp_array)
else:
q_interp = q0.interp(q1, alpha)
# Interpolate translation
pos_interp = [
x + (x1 - x) * alpha,
y + (y1 - y) * alpha,
z + (z1 - z) * alpha
]
# Compose SE3 transform
cart_traj.append(sm.SE3(pos_interp) * q_interp.SE3())'''
if self.desired[-1]:
timestamps = self.trapezoidal_timestamps(steps, self.desired[-1])
timestamps = self.trapezoidal_timestamps(steps, self.desired[-1], 0.8)
print(f'timestamps: {timestamps}')
for j in range(steps):
sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
if sol[1] == 1:
@@ -553,12 +586,12 @@ class OSC_ROS2_interface(Node):
else:
prev_duration = 0
'''
if self.prev_pose == None:
if self.previous_desired == None:
[x,y,z] = self.robot.fkine(self.current_joint_positions).t
[roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
else:
[x,y,z] = self.prev_pose[:3]
[roll, pitch, yaw] = self.prev_pose[3:]
[x,y,z] = self.previous_desired[:3]
[roll, pitch, yaw] = self.previous_desired[3:]
'''
for j in range(steps):
sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
@@ -609,32 +642,41 @@ class OSC_ROS2_interface(Node):
def send_joint_trajectory(self):
pass
self.previous_desired = None
def send_cartesian_trajectory(self):
pass
self.previous_desired = None
def update_position(self):
"""Calls the appropriate function to update the robot's position."""
try:
if self.desired is None or not(self.new):
return
start_time = time.time()
if self.desired[0] == "joint_positions":
self.new = False
self.send_joint_positions()
return
#return
elif self.desired[0] == "tcp_coordinates":
self.new = False
self.send_tcp_coordinates()
return
#return
elif self.desired[0] == "joint_trajectory":
self.new = False
self.send_joint_trajectory()
return
#return
elif self.desired[0] == "cartesian_trajectory":
self.new = False
self.send_cartesian_trajectory()
#return
else:
self.get_logger().warn(f"update_position: Unknown desired type '{self.desired[0]}'.")
return
self.latency.append(time.time()-start_time)
except Exception as e:
self.get_logger().fatal(f'update_position: {e}')
@@ -746,6 +788,7 @@ def main():
break
print("Invalid input. Please enter 'y' or 'n'.")
node = OSC_ROS2_interface(joint_names, joint_velocity_limits, robot, cost_mask)
# Run ROS 2 spin, and osc_process will be handled by the timer
@@ -754,6 +797,17 @@ def main():
except KeyboardInterrupt:
print("")
finally:
file_path = "./latency_log.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('latency')
with open(file_path, mode='a', newline='') as f:
writer = csv.writer(f)
for i in node.latency:
writer.writerow([i])
node.destroy_node()
rclpy.shutdown()
osc_terminate()

770
workspace/src/osc_ros2/osc_ros2/osc_ros2_latency.py Normal file
View File

@@ -0,0 +1,770 @@
import rclpy
from rclpy.node import Node
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from sensor_msgs.msg import JointState
from rcl_interfaces.msg import Log
from osc4py3.as_allthreads import *
from osc4py3 import oscmethod as osm
import xml.etree.ElementTree as ET
import numpy as np
import spatialmath as sm
import roboticstoolbox as rtb
from osc4py3 import oscbuildparse
import time
import os
import re
import socket
import csv
class JointNameListener(Node):
def __init__(self):
super().__init__('joint_name_listener')
self.subscription = self.create_subscription(
JointState,
'/joint_states',
self.joint_state_callback,
1
)
self.joint_names = None
def joint_state_callback(self, msg: JointState):
print("Joint names received from JointState message:")
self.joint_names = list(msg.name)
class OSC_ROS2_interface(Node):
"""Node to publish joint trajectories based on OSC messages."""
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,
self.trajectory_topic_name,
1
)
self.subscription = self.create_subscription(
JointState,
'/joint_states',
self.joint_states_callback,
1
)
self.subscription = self.create_subscription(
Log,
'/rosout',
self.log_callback,
100
)
# Store received joint positions
self.current_joint_positions = None
self.joint_names = joint_names
self.joint_velocity_limits = joint_velocity_limits
self.cost_mask = cost_mask
self.robot = robot
self.desired = None
self.previous_desired = None
self.log_dict = {
10: "DEBUG",
20: "INFO",
30: "WARN",
40: "ERROR",
50: "FATAL",
}
self.speed_scaling = 0.2
self.new = False
self.latency = []
self.time1 = None
while True:
try:
print('+-' * 50)
log_ip = str(input("Enter the target IP on which you want to recieve the log as OSC Messages (or press Enter for default: '127.0.0.1'): "))
if log_ip == "":
log_ip = "127.0.0.1"
print('--' * 50)
log_port = input("Enter the target port for the log messages (or press Enter for default: 5005): ")
if log_port == "":
log_port = 5005
else:
log_port = int(log_port)
break
except ValueError:
print("Invalid input. Please enter a valid IP address.")
continue
while True:
try:
print('+-' * 50)
state_ip = str(input("Enter the target IP on which you want to recieve the joint states as OSC Messages (or press Enter for default: '127.0.0.1'): "))
if state_ip == "":
state_ip = "127.0.0.1"
print('--' * 50)
state_port = input("Enter the target port for the log messages (or press Enter for default: 7000): ")
if state_port == "":
state_port = 7000
else:
state_port = int(state_port)
break
except ValueError:
print("Invalid input. Please enter a valid IP address.")
continue
while True:
try:
print('+-' * 50)
commands_port = input("Enter the port you want to send your commands to (or press Enter for default: 8000): ")
if commands_port == "":
commands_port = 8000
else:
commands_port = int(commands_port)
break
except ValueError:
print("Invalid input. Please enter a valid port.")
continue
if robot:
while True:
set_limits = input("Do you want to set limit for x, y and z? (y/n): ").strip().lower()
if set_limits == 'y':
while True:
try:
self.x_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for x (space-separated, enter 'x' for no limit): ").split()]
self.y_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for y (space-separated, enter 'x' for no limit): ").split()]
self.z_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for z (space-separated, enter 'x' for no limit): ").split()]
if len(self.x_limits) != 2 or len(self.y_limits) != 2 or len(self.z_limits) != 2:
print("Invalid input. Please enter exactly two values (or leave blank) for each limit.")
continue
if (self.x_limits[0] is not None and self.x_limits[1] is not None and self.x_limits[0] >= self.x_limits[1]) or \
(self.y_limits[0] is not None and self.y_limits[1] is not None and self.y_limits[0] >= self.y_limits[1]) or \
(self.z_limits[0] is not None and self.z_limits[1] is not None and self.z_limits[0] >= self.z_limits[1]):
print("Invalid input. Lower limit must be less than upper limit for each axis.")
continue
print(f"Current limits:")
print(f"x: {self.x_limits}")
print(f"y: {self.y_limits}")
print(f"z: {self.z_limits}")
con = True
while con:
confirm = input("Do you want your robot to move in this range? (y/n): ").strip().lower()
if confirm == 'y':
break
elif confirm == 'n':
print("Please re-enter the limits.")
con = False
else:
print("Invalid input. Please enter 'y' or 'n'.")
if con: break
except ValueError:
print("Invalid input. Please enter numeric values only.")
break
elif set_limits == 'n':
self.x_limits = [None, None]
self.y_limits = [None, None]
self.z_limits = [None, None]
break
print("Invalid input. Please enter 'y' or 'n'.")
# Ask the user if they want to set new joint limits
while True:
update_limits = input("Do you want to set new joint limits? (y/n): ").strip().lower()
if update_limits == 'y':
for i in range(len(self.joint_names)):
while True:
try:
lim = self.robot.qlim.copy()
# Find the link corresponding to the joint name
print("-" * 50)
print(f"Current position limits for joint '{self.joint_names[i]}': [{self.robot.qlim[0][i]} {self.robot.qlim[1][i]}] rad")
lower_limit = input(f"Enter the new lower limit for joint '{self.joint_names[i]}' (or press Enter to keep current): ").strip()
upper_limit = input(f"Enter the new upper limit for joint '{self.joint_names[i]}' (or press Enter to keep current): ").strip()
if lower_limit and upper_limit and float(lower_limit) >= float(upper_limit):
print("Invalid input. Lower limit must be less than upper limit.")
continue
if 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:
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)
break
except ValueError:
print("Invalid input. Please enter numeric values or leave blank to keep current limits.")
break
if update_limits == 'n':
break
print("Invalid input. Please enter 'y' or 'n'.")
'''
use_link_mask = input("Do you want to use a link mask? (y/n): ").strip().lower()
if use_link_mask == 'y':
while True:
try:
'''
else:
if not(self.joint_names):
while True:
print('-+'*50)
print("Joint names:")
print(self.joint_names)
print('-'*50)
correct = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: (y/n)?: ").strip()
if correct.lower() == 'y':
break
elif correct.lower() == 'n':
while True:
joint_names = []
print('-+'*50)
print("Enter the joint names manually one by one. Type 'done' when you are finished:")
print("Attention: The order of the joints is important. It should be the same in which you want to send the joint positions.")
while True:
print('-'*50)
joint_name = input("Enter joint name (or 'done' to finish): ").strip()
if joint_name.lower() == 'done':
break
if joint_name:
joint_names.append(joint_name)
print('-+'*50)
correct = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: {joint_names}. (y/n)?: ").strip()
if correct.lower() == 'y':
break
else:
print("Please re-enter the joint names.")
print('invalid input. Please enter "y" or "n".')
self.n_joints = len(joint_names)
osc_startup()
osc_udp_client(state_ip, state_port, "osc_client")
osc_udp_client(log_ip, log_port, "osc_log_client")
osc_udp_server('0.0.0.0', commands_port, "osc_server")
# 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("/tcp_coordinates", self.tcp_coordinates_handler, argscheme=osm.OSCARG_DATAUNPACK+osm.OSCARG_READTIME)
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()
print('=-=' * 50)
print()
print(f'Sending joint states to {state_ip}:{state_port}')
print()
print('=-=' * 50)
print()
print(f'Sending log messages to {log_ip}:{log_port}')
print()
print('=-=' * 50)
print()
print(f'Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{commands_port}')
print()
print('=-=' * 50)
print()
self.get_logger().info(f'Ready to receive OSC messages on 0.0.0.0:{commands_port}')
self.get_logger().info(f'Sending joint states to {state_ip}:{state_port}')
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
self.create_timer(3, self.reset_prev) # reset the previous desired position
def reset_prev(self): self.previous_desired = None
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:
pass
else:
self.get_logger().warn("cartesian_trajectory_handler: No robot model provided. Cannot handle cartesian trajectory.")
return
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
# 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
self.new = True
except Exception as e:
self.get_logger().fatal(f"joint_positions_handler: {e}")
return
def tcp_coordinates_handler(self, *args):
# Ensure the desired joint positions are within the specified limits
"""Handles incoming OSC messages for TCP coordinates."""
print(args)
recv_time = args[-1]
if self.robot:
try:
if len(args) == 6:
x, y, z, r, p, yaw = [float(i) for i in list(args)]
duration = None
elif len(args) >= 7:
x, y, z, r, p, yaw, duration, *_ = [float(i) for i in list(args)]
else:
self.get_logger().warn(f"tcp_coordinates_handler: Invalid number of arguments for TCP coordinates. Expected 6 ([x, y, z, roll, pitch, yaw]) or 7 ([x, y, z, roll, pitch, yaw, duration]), but got {len(args)}.")
return
if self.x_limits[0] is not None:
x = max(self.x_limits[0], x)
if self.x_limits[1] is not None:
x = min(self.x_limits[1], x)
if self.y_limits[0] is not None:
y = max(self.y_limits[0], y)
if self.y_limits[1] is not None:
y = min(self.y_limits[1], y)
if self.z_limits[0] is not None:
z = max(self.z_limits[0], z)
if self.z_limits[1] is not None:
z = min(self.z_limits[1], z)
if x != args[0] or y != args[1] or z != args[2]:
self.get_logger().warn(
f"tcp_coordinates_handler: Desired joint positions adjusted to fit within limits: "
f"x={x}, y={y}, z={z} (original: x={args[0]}, y={args[1]}, z={args[2]})"
)
self.desired = ["tcp_coordinates", x, y, z, r, p, yaw, duration, recv_time]
self.new = True
except Exception as e:
self.get_logger().fatal(f"tcp_coordinates_handler: {e}")
else:
self.get_logger().warn("tcp_coordinates_handler: No robot model provided. Cannot handle TCP coordinates.")
return
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]
joint_position_dict = dict(zip(msg.name, msg.velocity))
self.current_joint_velocities = [joint_position_dict[name] for name in self.joint_names]
if self.robot:
tcp_position = self.robot.fkine(self.current_joint_positions).t
tcp_orientation = self.robot.fkine(self.current_joint_positions).rpy()
msg_tcp = oscbuildparse.OSCMessage(f"/tcp_coordinates", ',ffffff', [tcp_position[0], tcp_position[1], tcp_position[2], tcp_orientation[0], tcp_orientation[1], tcp_orientation[2]])
msg_x = oscbuildparse.OSCMessage(f"/tcp_coordinates/x", ',f', [tcp_position[0]])
msg_y = oscbuildparse.OSCMessage(f"/tcp_coordinates/y", ',f', [tcp_position[1]])
msg_z = oscbuildparse.OSCMessage(f"/tcp_coordinates/z", ',f', [tcp_position[2]])
msg_roll = oscbuildparse.OSCMessage(f"/tcp_coordinates/roll", ',f', [tcp_orientation[0]])
msg_pitch = oscbuildparse.OSCMessage(f"/tcp_coordinates/pitch", ',f', [tcp_orientation[1]])
msg_yaw = oscbuildparse.OSCMessage(f"/tcp_coordinates/yaw", ',f', [tcp_orientation[2]])
bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_tcp, msg_x, msg_y, msg_z, msg_roll, msg_pitch, msg_yaw])
osc_send(bun, "osc_client")
msg_position = oscbuildparse.OSCMessage(f"/joint_state/position", f',{"f"*self.n_joints}', [i for i in msg.position])
msg_velocity = oscbuildparse.OSCMessage(f"/joint_state/velocity", f',{"f"*self.n_joints}', [i for i in msg.velocity])
msg_effort = oscbuildparse.OSCMessage(f"/joint_state/effort", f',{"f"*self.n_joints}', [i for i in msg.effort])
msg_name = oscbuildparse.OSCMessage(f"/joint_state/name", f',{"s"*self.n_joints}', [i for i in msg.name])
bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_name, msg_position, msg_velocity, msg_effort])
osc_send(bun, "osc_client")
for i, name in enumerate(msg.name):
msg_position = oscbuildparse.OSCMessage(f"/joint_state/position/{name}", ',f', [msg.position[i]])
msg_velocity = oscbuildparse.OSCMessage(f"/joint_state/velocity/{name}", ',f', [msg.velocity[i]])
msg_effort = oscbuildparse.OSCMessage(f"/joint_state/effort/{name}", ',f', [msg.effort[i]])
bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_position, msg_velocity, msg_effort])
osc_send(bun, "osc_client")
except Exception as e:
self.get_logger().fatal(f"joint_states_callback: {e}")
def send_joint_positions(self):
pass
self.previous_desired = None
def trapezoidal_timestamps(self, num_points,total_duration, flat_ratio = 0.3):
if num_points == 2:
return [0, total_duration]
n = int(num_points*(1-flat_ratio)/2)
start = np.cos(np.linspace(0, np.pi, n))+2
end = np.cos(np.linspace(-np.pi, 0, n))+2
flat = np.ones(num_points-2*n)
timestamps = np.concatenate((start, flat, end))
timestamps *= total_duration / timestamps.sum()
timestamps = np.cumsum(timestamps)
return timestamps.tolist()
def send_tcp_coordinates(self):
"""Send the desired TCP coordinates to the robot."""
try:
msg = JointTrajectory()
msg.joint_names = self.joint_names
steps_per_m = 100
if self.previous_desired == None:
[x,y,z] = self.robot.fkine(self.current_joint_positions).t
[roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
else:
[x,y,z] = self.previous_desired[1:4]
[roll, pitch, yaw] = self.previous_desired[4:7]
x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7]
self.previous_desired = self.desired
steps = int(np.linalg.norm(np.array([x1, y1, z1])- self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
if steps < 2: steps = 2
cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)]
if False:#self.desired[-2]:
print(f"self.desired: {self.desired}")
timestamps = self.trapezoidal_timestamps(steps, self.desired[-2], 0.8)
for j in range(steps):
sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
if sol[1] == 1:
fowards = self.robot.fkine_all(sol[0])
out_of_bounds = (fowards.t[1:,0] > self.x_limits[1] if self.x_limits[1] != None else False) | (fowards.t[1:,0] < self.x_limits[0] if self.x_limits[0] != None else False) | (fowards.t[1:,1] > self.y_limits[1] if self.y_limits[1] != None else False) | (fowards.t[1:,1] < self.y_limits[0] if self.y_limits[0] != None else False) | (fowards.t[1:,2] > self.z_limits[1] if self.z_limits[1] != None else False) | (fowards.t[1:,2] < self.z_limits[0] if self.z_limits[0] != None else False)
if np.any(out_of_bounds):
#print(fowards.t)
#indices = np.where(out_of_bounds)[0]
#print(f"indices: {indices}")
self.get_logger().warn("send_tcp_coordinates: One or more links moved out of bounds!")
'''
for i in indices:
try:
print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}")
except IndexError:
print(f"index {i} is out of bounds, but no corresponding joint found.")
self.previous_desired_tcp_position = self.desired_tcp_position
'''
break
duration = timestamps[j]
if duration == 0:
prev_sol = list(sol[0])
continue
point = JointTrajectoryPoint()
point.positions = list(sol[0])
point.time_from_start.sec = int(duration)
point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
msg.points.append(point)
prev_sol = list(sol[0])
else:
self.get_logger().warn(f"send_tcp_coordinates: IK could not find a solution for (x,y,z) = {cart_traj[j].t} and (r,p,y) = {cart_traj[j].rpy()}!")
prev_sol = self.current_joint_positions
if len(msg.points) == 0:
self.get_logger().warn("send_tcp_coordinates: The resulting trajectory is empty. Either the IK failed or the trajectory is too short.")
self.previous_desired = self.desired
return
msg.header.stamp = self.get_clock().now().to_msg()
self.publisher.publish(msg)
self.previous_desired = self.desired
else:
prev_duration = 0
'''
if self.previous_desired == None:
[x,y,z] = self.robot.fkine(self.current_joint_positions).t
[roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
else:
[x,y,z] = self.previous_desired[:3]
[roll, pitch, yaw] = self.previous_desired[3:]
'''
for j in range(steps):
sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
if sol[1] == 1:
fowards = self.robot.fkine_all(sol[0])
out_of_bounds = (fowards.t[1:,0] > self.x_limits[1] if self.x_limits[1] != None else False) | (fowards.t[1:,0] < self.x_limits[0] if self.x_limits[0] != None else False) | (fowards.t[1:,1] > self.y_limits[1] if self.y_limits[1] != None else False) | (fowards.t[1:,1] < self.y_limits[0] if self.y_limits[0] != None else False) | (fowards.t[1:,2] > self.z_limits[1] if self.z_limits[1] != None else False) | (fowards.t[1:,2] < self.z_limits[0] if self.z_limits[0] != None else False)
if np.any(out_of_bounds):
#print(fowards.t)
#indices = np.where(out_of_bounds)[0]
#print(f"indices: {indices}")
self.get_logger().warn("send_tcp_coordinates: One or more links moved out of bounds!")
'''
for i in indices:
try:
print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}")
except IndexError:
print(f"index {i} is out of bounds, but no corresponding joint found.")
self.previous_desired_tcp_position = self.desired_tcp_position
'''
break
duration = 0
prev = self.current_joint_positions if j == 0 else prev_sol
for p1, p2, max_vel in zip(sol[0], prev, self.joint_velocity_limits.values()):
duration = max(duration, abs(p1 - p2) / max_vel)#, 1/self.hz) # as minimun
prev_sol = list(sol[0])
if duration == 0:
continue
point = JointTrajectoryPoint()
point.positions = list(sol[0])
duration /= self.speed_scaling
duration += prev_duration
prev_duration = duration
point.time_from_start.sec = int(duration)
point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
msg.points.append(point)
else:
self.get_logger().warn(f"send_tcp_coordinates: IK could not find a solution for (x,y,z) = {cart_traj[j].t} and (r,p,y) = {cart_traj[j].rpy()}!")
prev_sol = self.current_joint_positions
if len(msg.points) == 0:
self.get_logger().warn("send_tcp_coordinates: The resulting trajectory is empty. Either the IK failed or the trajectory is too short.")
self.previous_desired = self.desired
return
msg.header.stamp = self.get_clock().now().to_msg()
self.publisher.publish(msg)
self.previous_desired = self.desired
except Exception as e:
self.get_logger().fatal(f"send_tcp_coordinates: {e}")
def send_joint_trajectory(self):
pass
self.previous_desired = None
def send_cartesian_trajectory(self):
pass
self.previous_desired = None
def update_position(self):
"""Calls the appropriate function to update the robot's position."""
try:
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()
self.latency.append(time.time()-self.desired[-1])
#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
else:
self.get_logger().warn(f"update_position: Unknown desired type '{self.desired[0]}'.")
return
except Exception as e:
self.get_logger().fatal(f'update_position: {e}')
def clean_log_string(self, s):
s = str(s)
# Remove ANSI escape sequences (e.g., \x1b[31m)
ansi_escape = re.compile(r'\x1B(?:[@-Z\\-_]|\[[0-?]*[ -/]*[@-~])')
s = ansi_escape.sub('', s)
# Replace tabs/newlines with spaces
s = s.replace('\n', ' ').replace('\r', ' ').replace('\t', ' ').replace("'", ' '). replace('"', ' ').replace('`', ' ').replace('´', ' ').replace('`', ' ').replace('“', ' ').replace('”', ' ').replace('‘', ' ').replace('’', ' ').replace('´', ' ').replace('`', ' ').replace('“', ' ').replace('”', ' ').replace('‘', ' ').replace('’', ' ')
# Strip leading/trailing whitespace
s = s.strip()
# Optionally enforce ASCII only (replace non-ASCII chars with '?')
s = s.encode('ascii', 'replace').decode('ascii')
return s
def log_callback(self, msg: Log):
"""Callback function to handle incoming log messages."""
# Send the log message as an OSC message
msg_log = oscbuildparse.OSCMessage(f"/log/{self.log_dict.get(msg.level, 'UNKNOWN')}", ',isss', [int(msg.level), str(msg.stamp.sec+msg.stamp.nanosec*1e-9) , str(msg.name), self.clean_log_string(msg.msg)])
osc_send(msg_log, "osc_log_client")
def main():
"""Main function to get joint names and start the ROS 2 & OSC system."""
rclpy.init()
while True:
use_urdf = input("Do you have a URDF file you want to use? (y/n): ").strip().lower()
if use_urdf == 'y':
while True:
robot_urdf = input("Enter the path to the URDF file: ")
if os.path.isfile(robot_urdf):
if not robot_urdf.endswith('.urdf'):
print("The file is not a URDF file. Please enter a valid URDF file.")
continue
break
else:
print("Invalid path. Please enter a valid path to the URDF file.")
tree = ET.parse(robot_urdf)
root = tree.getroot()
robot = rtb.ERobot.URDF(robot_urdf)
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']
print(robot)
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('-+'*50)
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}")
break
elif use_urdf == 'n':
node = JointNameListener()
print("Wainting 10 sec for JointState messages to extract joint names...")
rclpy.spin_once(node)
counter = 0
while not(node.joint_names):
if counter > 100:
joint_names = None
break
counter+=1
time.sleep(0.1)
rclpy.spin_once(node)
joint_names = node.joint_names
node.destroy_node()
'''
if joint_names:
while True:
try:
joint_velocity_limits = {name: float(input(f"Enter the velocity limit for joint '{name}' (or press Enter to skip): ").strip())} for name in joint_names}
break
except ValueError:
print("Invalid input. Please enter numeric values or leave blank to skip.")
'''
joint_velocity_limits = None
robot = None
cost_mask = None
break
print("Invalid input. Please enter 'y' or 'n'.")
node = OSC_ROS2_interface(joint_names, joint_velocity_limits, robot, cost_mask)
# Run ROS 2 spin, and osc_process will be handled by the timer
try:
rclpy.spin(node)
except KeyboardInterrupt:
print("")
finally:
file_path = "./latency_log.csv"
with open(file_path, mode='a', newline='') as f:
writer = csv.writer(f)
writer.writerow('latency')
for i in node.latency:
writer.writerow([i])
node.destroy_node()
rclpy.shutdown()
osc_terminate()
if __name__ == '__main__':
main()

811
workspace/src/osc_ros2/osc_ros2/osc_ros2_osc_record.py Normal file
View File

@@ -0,0 +1,811 @@
import rclpy
from rclpy.node import Node
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from sensor_msgs.msg import JointState
from rcl_interfaces.msg import Log
from osc4py3.as_allthreads import *
from osc4py3 import oscmethod as osm
import xml.etree.ElementTree as ET
import numpy as np
import spatialmath as sm
import roboticstoolbox as rtb
from osc4py3 import oscbuildparse
import time
import os
import re
import socket
import csv
class JointNameListener(Node):
def __init__(self):
super().__init__('joint_name_listener')
self.subscription = self.create_subscription(
JointState,
'/joint_states',
self.joint_state_callback,
1
)
self.joint_names = None
def joint_state_callback(self, msg: JointState):
print("Joint names received from JointState message:")
self.joint_names = list(msg.name)
class OSC_ROS2_interface(Node):
"""Node to publish joint trajectories based on OSC messages."""
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,
self.trajectory_topic_name,
1
)
self.subscription = self.create_subscription(
JointState,
'/joint_states',
self.joint_states_callback,
1
)
self.subscription = self.create_subscription(
Log,
'/rosout',
self.log_callback,
100
)
# Store received joint positions
self.current_joint_positions = None
self.joint_names = joint_names
self.joint_velocity_limits = joint_velocity_limits
self.cost_mask = cost_mask
self.robot = robot
self.desired = None
self.previous_desired = None
self.log_dict = {
10: "DEBUG",
20: "INFO",
30: "WARN",
40: "ERROR",
50: "FATAL",
}
self.speed_scaling = 0.2
self.new = False
self.commands =[]
while True:
try:
print('+-' * 50)
log_ip = str(input("Enter the target IP on which you want to recieve the log as OSC Messages (or press Enter for default: '127.0.0.1'): "))
if log_ip == "":
log_ip = "127.0.0.1"
print('--' * 50)
log_port = input("Enter the target port for the log messages (or press Enter for default: 5005): ")
if log_port == "":
log_port = 5005
else:
log_port = int(log_port)
break
except ValueError:
print("Invalid input. Please enter a valid IP address.")
continue
while True:
try:
print('+-' * 50)
state_ip = str(input("Enter the target IP on which you want to recieve the joint states as OSC Messages (or press Enter for default: '127.0.0.1'): "))
if state_ip == "":
state_ip = "127.0.0.1"
print('--' * 50)
state_port = input("Enter the target port for the log messages (or press Enter for default: 7000): ")
if state_port == "":
state_port = 7000
else:
state_port = int(state_port)
break
except ValueError:
print("Invalid input. Please enter a valid IP address.")
continue
while True:
try:
print('+-' * 50)
commands_port = input("Enter the port you want to send your commands to (or press Enter for default: 8000): ")
if commands_port == "":
commands_port = 8000
else:
commands_port = int(commands_port)
break
except ValueError:
print("Invalid input. Please enter a valid port.")
continue
if robot:
while True:
set_limits = input("Do you want to set limit for x, y and z? (y/n): ").strip().lower()
if set_limits == 'y':
while True:
try:
self.x_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for x (space-separated, enter 'x' for no limit): ").split()]
self.y_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for y (space-separated, enter 'x' for no limit): ").split()]
self.z_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for z (space-separated, enter 'x' for no limit): ").split()]
if len(self.x_limits) != 2 or len(self.y_limits) != 2 or len(self.z_limits) != 2:
print("Invalid input. Please enter exactly two values (or leave blank) for each limit.")
continue
if (self.x_limits[0] is not None and self.x_limits[1] is not None and self.x_limits[0] >= self.x_limits[1]) or \
(self.y_limits[0] is not None and self.y_limits[1] is not None and self.y_limits[0] >= self.y_limits[1]) or \
(self.z_limits[0] is not None and self.z_limits[1] is not None and self.z_limits[0] >= self.z_limits[1]):
print("Invalid input. Lower limit must be less than upper limit for each axis.")
continue
print(f"Current limits:")
print(f"x: {self.x_limits}")
print(f"y: {self.y_limits}")
print(f"z: {self.z_limits}")
con = True
while con:
confirm = input("Do you want your robot to move in this range? (y/n): ").strip().lower()
if confirm == 'y':
break
elif confirm == 'n':
print("Please re-enter the limits.")
con = False
else:
print("Invalid input. Please enter 'y' or 'n'.")
if con: break
except ValueError:
print("Invalid input. Please enter numeric values only.")
break
elif set_limits == 'n':
self.x_limits = [None, None]
self.y_limits = [None, None]
self.z_limits = [None, None]
break
print("Invalid input. Please enter 'y' or 'n'.")
# Ask the user if they want to set new joint limits
while True:
update_limits = input("Do you want to set new joint limits? (y/n): ").strip().lower()
if update_limits == 'y':
for i in range(len(self.joint_names)):
while True:
try:
lim = self.robot.qlim.copy()
# Find the link corresponding to the joint name
print("-" * 50)
print(f"Current position limits for joint '{self.joint_names[i]}': [{self.robot.qlim[0][i]} {self.robot.qlim[1][i]}] rad")
lower_limit = input(f"Enter the new lower limit for joint '{self.joint_names[i]}' (or press Enter to keep current): ").strip()
upper_limit = input(f"Enter the new upper limit for joint '{self.joint_names[i]}' (or press Enter to keep current): ").strip()
if lower_limit and upper_limit and float(lower_limit) >= float(upper_limit):
print("Invalid input. Lower limit must be less than upper limit.")
continue
if 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:
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)
break
except ValueError:
print("Invalid input. Please enter numeric values or leave blank to keep current limits.")
break
if update_limits == 'n':
break
print("Invalid input. Please enter 'y' or 'n'.")
'''
use_link_mask = input("Do you want to use a link mask? (y/n): ").strip().lower()
if use_link_mask == 'y':
while True:
try:
'''
else:
if not(self.joint_names):
while True:
print('-+'*50)
print("Joint names:")
print(self.joint_names)
print('-'*50)
correct = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: (y/n)?: ").strip()
if correct.lower() == 'y':
break
elif correct.lower() == 'n':
while True:
joint_names = []
print('-+'*50)
print("Enter the joint names manually one by one. Type 'done' when you are finished:")
print("Attention: The order of the joints is important. It should be the same in which you want to send the joint positions.")
while True:
print('-'*50)
joint_name = input("Enter joint name (or 'done' to finish): ").strip()
if joint_name.lower() == 'done':
break
if joint_name:
joint_names.append(joint_name)
print('-+'*50)
correct = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: {joint_names}. (y/n)?: ").strip()
if correct.lower() == 'y':
break
else:
print("Please re-enter the joint names.")
print('invalid input. Please enter "y" or "n".')
self.n_joints = len(joint_names)
osc_startup()
osc_udp_client(state_ip, state_port, "osc_client")
osc_udp_client(log_ip, log_port, "osc_log_client")
osc_udp_server('0.0.0.0', commands_port, "osc_server")
# 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("/tcp_coordinates", self.tcp_coordinates_handler, argscheme=osm.OSCARG_READTIME+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()
print('=-=' * 50)
print()
print(f'Sending joint states to {state_ip}:{state_port}')
print()
print('=-=' * 50)
print()
print(f'Sending log messages to {log_ip}:{log_port}')
print()
print('=-=' * 50)
print()
print(f'Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{commands_port}')
print()
print('=-=' * 50)
print()
self.get_logger().info(f'Ready to receive OSC messages on 0.0.0.0:{commands_port}')
self.get_logger().info(f'Sending joint states to {state_ip}:{state_port}')
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
self.create_timer(3, self.reset_prev) # reset the previous desired position
def reset_prev(self): self.previous_desired = None
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:
pass
else:
self.get_logger().warn("cartesian_trajectory_handler: No robot model provided. Cannot handle cartesian trajectory.")
return
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
# 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
self.new = True
except Exception as e:
self.get_logger().fatal(f"joint_positions_handler: {e}")
return
def tcp_coordinates_handler(self, readtime, *args):
# Ensure the desired joint positions are within the specified limits
if self.robot:
try:
self.commands.append([readtime] + list(args))
if len(args) == 6:
x, y, z, r, p, yaw = [float(i) for i in list(args)]
duration = None
elif len(args) >= 7:
x, y, z, r, p, yaw, duration, *_ = [float(i) for i in list(args)]
else:
self.get_logger().warn(f"tcp_coordinates_handler: Invalid number of arguments for TCP coordinates. Expected 6 ([x, y, z, roll, pitch, yaw]) or 7 ([x, y, z, roll, pitch, yaw, duration]), but got {len(args)}.")
return
if self.x_limits[0] is not None:
x = max(self.x_limits[0], x)
if self.x_limits[1] is not None:
x = min(self.x_limits[1], x)
if self.y_limits[0] is not None:
y = max(self.y_limits[0], y)
if self.y_limits[1] is not None:
y = min(self.y_limits[1], y)
if self.z_limits[0] is not None:
z = max(self.z_limits[0], z)
if self.z_limits[1] is not None:
z = min(self.z_limits[1], z)
if x != args[0] or y != args[1] or z != args[2]:
self.get_logger().warn(
f"tcp_coordinates_handler: Desired joint positions adjusted to fit within limits: "
f"x={x}, y={y}, z={z} (original: x={args[0]}, y={args[1]}, z={args[2]})"
)
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:
self.get_logger().warn("tcp_coordinates_handler: No robot model provided. Cannot handle TCP coordinates.")
return
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]
joint_position_dict = dict(zip(msg.name, msg.velocity))
self.current_joint_velocities = [joint_position_dict[name] for name in self.joint_names]
if self.robot:
tcp_position = self.robot.fkine(self.current_joint_positions).t
tcp_orientation = self.robot.fkine(self.current_joint_positions).rpy()
msg_tcp = oscbuildparse.OSCMessage(f"/tcp_coordinates", ',ffffff', [tcp_position[0], tcp_position[1], tcp_position[2], tcp_orientation[0], tcp_orientation[1], tcp_orientation[2]])
msg_x = oscbuildparse.OSCMessage(f"/tcp_coordinates/x", ',f', [tcp_position[0]])
msg_y = oscbuildparse.OSCMessage(f"/tcp_coordinates/y", ',f', [tcp_position[1]])
msg_z = oscbuildparse.OSCMessage(f"/tcp_coordinates/z", ',f', [tcp_position[2]])
msg_roll = oscbuildparse.OSCMessage(f"/tcp_coordinates/roll", ',f', [tcp_orientation[0]])
msg_pitch = oscbuildparse.OSCMessage(f"/tcp_coordinates/pitch", ',f', [tcp_orientation[1]])
msg_yaw = oscbuildparse.OSCMessage(f"/tcp_coordinates/yaw", ',f', [tcp_orientation[2]])
bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_tcp, msg_x, msg_y, msg_z, msg_roll, msg_pitch, msg_yaw])
osc_send(bun, "osc_client")
msg_position = oscbuildparse.OSCMessage(f"/joint_state/position", f',{"f"*self.n_joints}', [i for i in msg.position])
msg_velocity = oscbuildparse.OSCMessage(f"/joint_state/velocity", f',{"f"*self.n_joints}', [i for i in msg.velocity])
msg_effort = oscbuildparse.OSCMessage(f"/joint_state/effort", f',{"f"*self.n_joints}', [i for i in msg.effort])
msg_name = oscbuildparse.OSCMessage(f"/joint_state/name", f',{"s"*self.n_joints}', [i for i in msg.name])
bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_name, msg_position, msg_velocity, msg_effort])
osc_send(bun, "osc_client")
for i, name in enumerate(msg.name):
msg_position = oscbuildparse.OSCMessage(f"/joint_state/position/{name}", ',f', [msg.position[i]])
msg_velocity = oscbuildparse.OSCMessage(f"/joint_state/velocity/{name}", ',f', [msg.velocity[i]])
msg_effort = oscbuildparse.OSCMessage(f"/joint_state/effort/{name}", ',f', [msg.effort[i]])
bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_position, msg_velocity, msg_effort])
osc_send(bun, "osc_client")
except Exception as e:
self.get_logger().fatal(f"joint_states_callback: {e}")
def send_joint_positions(self):
pass
self.previous_desired = None
def trapezoidal_timestamps(self, num_points,total_duration, flat_ratio = 0.3):
if num_points == 2:
return [0, total_duration]
n = int(num_points*(1-flat_ratio)/2)
start = np.cos(np.linspace(0, np.pi, n))+2
end = np.cos(np.linspace(-np.pi, 0, n))+2
flat = np.ones(num_points-2*n)
timestamps = np.concatenate((start, flat, end))
timestamps *= total_duration / timestamps.sum()
timestamps = np.cumsum(timestamps)
return timestamps.tolist()
def send_tcp_coordinates(self):
"""Send the desired TCP coordinates to the robot."""
try:
msg = JointTrajectory()
msg.joint_names = self.joint_names
steps_per_m = 100
if self.previous_desired == None:
[x,y,z] = self.robot.fkine(self.current_joint_positions).t
[roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
else:
[x,y,z] = self.previous_desired[1:4]
[roll, pitch, yaw] = self.previous_desired[4:-1]
x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7]
self.previous_desired = self.desired
steps = int(np.linalg.norm(np.array([x1, y1, z1])- self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
if steps < 2: steps = 2
cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)]
'''if self.previous_desired:
[x,y,z] = self.previous_desired[1:4]
q0 = sm.UnitQuaternion.RPY(self.previous_desired[3], self.previous_desired[4], self.previous_desired[5])
else:
[x, y, z] = self.robot.fkine(self.current_joint_positions).t
q0 = sm.UnitQuaternion(self.robot.fkine(self.current_joint_positions).R)
x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7]
q1 = sm.UnitQuaternion.RPY(roll1, pitch1, yaw1)
steps = int(np.linalg.norm(np.array([x1, y1, z1]) - self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
if steps < 2:
steps = 2
cart_traj = []
for i in range(steps):
alpha = i / (steps - 1)
# Convert to arrays for robust interpolation if needed
q0_array = q0.vec
q1_array = q1.vec
dot = np.dot(q0_array, q1_array)
dot = np.clip(dot, -1.0, 1.0)
if abs(dot) > 0.9995:
# Linear interpolation + normalization
q_interp_array = (1 - alpha) * q0_array + alpha * q1_array
q_interp_array = q_interp_array / np.linalg.norm(q_interp_array)
q_interp = sm.UnitQuaternion(q_interp_array)
else:
q_interp = q0.interp(q1, alpha)
# Interpolate translation
pos_interp = [
x + (x1 - x) * alpha,
y + (y1 - y) * alpha,
z + (z1 - z) * alpha
]
# Compose SE3 transform
cart_traj.append(sm.SE3(pos_interp) * q_interp.SE3())'''
if self.desired[-1]:
timestamps = self.trapezoidal_timestamps(steps, self.desired[-1], 0.8)
print(f'timestamps: {timestamps}')
for j in range(steps):
sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
if sol[1] == 1:
fowards = self.robot.fkine_all(sol[0])
out_of_bounds = (fowards.t[1:,0] > self.x_limits[1] if self.x_limits[1] != None else False) | (fowards.t[1:,0] < self.x_limits[0] if self.x_limits[0] != None else False) | (fowards.t[1:,1] > self.y_limits[1] if self.y_limits[1] != None else False) | (fowards.t[1:,1] < self.y_limits[0] if self.y_limits[0] != None else False) | (fowards.t[1:,2] > self.z_limits[1] if self.z_limits[1] != None else False) | (fowards.t[1:,2] < self.z_limits[0] if self.z_limits[0] != None else False)
if np.any(out_of_bounds):
#print(fowards.t)
#indices = np.where(out_of_bounds)[0]
#print(f"indices: {indices}")
self.get_logger().warn("send_tcp_coordinates: One or more links moved out of bounds!")
'''
for i in indices:
try:
print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}")
except IndexError:
print(f"index {i} is out of bounds, but no corresponding joint found.")
self.previous_desired_tcp_position = self.desired_tcp_position
'''
break
duration = timestamps[j]
if duration == 0:
prev_sol = list(sol[0])
continue
point = JointTrajectoryPoint()
point.positions = list(sol[0])
point.time_from_start.sec = int(duration)
point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
msg.points.append(point)
prev_sol = list(sol[0])
else:
self.get_logger().warn(f"send_tcp_coordinates: IK could not find a solution for (x,y,z) = {cart_traj[j].t} and (r,p,y) = {cart_traj[j].rpy()}!")
prev_sol = self.current_joint_positions
if len(msg.points) == 0:
self.get_logger().warn("send_tcp_coordinates: The resulting trajectory is empty. Either the IK failed or the trajectory is too short.")
self.previous_desired = self.desired
return
msg.header.stamp = self.get_clock().now().to_msg()
self.publisher.publish(msg)
self.previous_desired = self.desired
else:
prev_duration = 0
'''
if self.previous_desired == None:
[x,y,z] = self.robot.fkine(self.current_joint_positions).t
[roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
else:
[x,y,z] = self.previous_desired[:3]
[roll, pitch, yaw] = self.previous_desired[3:]
'''
for j in range(steps):
sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
if sol[1] == 1:
fowards = self.robot.fkine_all(sol[0])
out_of_bounds = (fowards.t[1:,0] > self.x_limits[1] if self.x_limits[1] != None else False) | (fowards.t[1:,0] < self.x_limits[0] if self.x_limits[0] != None else False) | (fowards.t[1:,1] > self.y_limits[1] if self.y_limits[1] != None else False) | (fowards.t[1:,1] < self.y_limits[0] if self.y_limits[0] != None else False) | (fowards.t[1:,2] > self.z_limits[1] if self.z_limits[1] != None else False) | (fowards.t[1:,2] < self.z_limits[0] if self.z_limits[0] != None else False)
if np.any(out_of_bounds):
#print(fowards.t)
#indices = np.where(out_of_bounds)[0]
#print(f"indices: {indices}")
self.get_logger().warn("send_tcp_coordinates: One or more links moved out of bounds!")
'''
for i in indices:
try:
print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}")
except IndexError:
print(f"index {i} is out of bounds, but no corresponding joint found.")
self.previous_desired_tcp_position = self.desired_tcp_position
'''
break
duration = 0
prev = self.current_joint_positions if j == 0 else prev_sol
for p1, p2, max_vel in zip(sol[0], prev, self.joint_velocity_limits.values()):
duration = max(duration, abs(p1 - p2) / max_vel)#, 1/self.hz) # as minimun
prev_sol = list(sol[0])
if duration == 0:
continue
point = JointTrajectoryPoint()
point.positions = list(sol[0])
duration /= self.speed_scaling
duration += prev_duration
prev_duration = duration
point.time_from_start.sec = int(duration)
point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
msg.points.append(point)
else:
self.get_logger().warn(f"send_tcp_coordinates: IK could not find a solution for (x,y,z) = {cart_traj[j].t} and (r,p,y) = {cart_traj[j].rpy()}!")
prev_sol = self.current_joint_positions
if len(msg.points) == 0:
self.get_logger().warn("send_tcp_coordinates: The resulting trajectory is empty. Either the IK failed or the trajectory is too short.")
self.previous_desired = self.desired
return
msg.header.stamp = self.get_clock().now().to_msg()
self.publisher.publish(msg)
self.previous_desired = self.desired
except Exception as e:
self.get_logger().fatal(f"send_tcp_coordinates: {e}")
def send_joint_trajectory(self):
pass
self.previous_desired = None
def send_cartesian_trajectory(self):
pass
self.previous_desired = None
def update_position(self):
"""Calls the appropriate function to update the robot's position."""
try:
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
else:
self.get_logger().warn(f"update_position: Unknown desired type '{self.desired[0]}'.")
return
except Exception as e:
self.get_logger().fatal(f'update_position: {e}')
def clean_log_string(self, s):
s = str(s)
# Remove ANSI escape sequences (e.g., \x1b[31m)
ansi_escape = re.compile(r'\x1B(?:[@-Z\\-_]|\[[0-?]*[ -/]*[@-~])')
s = ansi_escape.sub('', s)
# Replace tabs/newlines with spaces
s = s.replace('\n', ' ').replace('\r', ' ').replace('\t', ' ').replace("'", ' '). replace('"', ' ').replace('`', ' ').replace('´', ' ').replace('`', ' ').replace('“', ' ').replace('”', ' ').replace('‘', ' ').replace('’', ' ').replace('´', ' ').replace('`', ' ').replace('“', ' ').replace('”', ' ').replace('‘', ' ').replace('’', ' ')
# Strip leading/trailing whitespace
s = s.strip()
# Optionally enforce ASCII only (replace non-ASCII chars with '?')
s = s.encode('ascii', 'replace').decode('ascii')
return s
def log_callback(self, msg: Log):
"""Callback function to handle incoming log messages."""
# Send the log message as an OSC message
msg_log = oscbuildparse.OSCMessage(f"/log/{self.log_dict.get(msg.level, 'UNKNOWN')}", ',isss', [int(msg.level), str(msg.stamp.sec+msg.stamp.nanosec*1e-9) , str(msg.name), self.clean_log_string(msg.msg)])
osc_send(msg_log, "osc_log_client")
def main():
"""Main function to get joint names and start the ROS 2 & OSC system."""
rclpy.init()
while True:
use_urdf = input("Do you have a URDF file you want to use? (y/n): ").strip().lower()
if use_urdf == 'y':
while True:
robot_urdf = input("Enter the path to the URDF file: ")
if os.path.isfile(robot_urdf):
if not robot_urdf.endswith('.urdf'):
print("The file is not a URDF file. Please enter a valid URDF file.")
continue
break
else:
print("Invalid path. Please enter a valid path to the URDF file.")
tree = ET.parse(robot_urdf)
root = tree.getroot()
robot = rtb.ERobot.URDF(robot_urdf)
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']
print(robot)
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('-+'*50)
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}")
break
elif use_urdf == 'n':
node = JointNameListener()
print("Wainting 10 sec for JointState messages to extract joint names...")
rclpy.spin_once(node)
counter = 0
while not(node.joint_names):
if counter > 100:
joint_names = None
break
counter+=1
time.sleep(0.1)
rclpy.spin_once(node)
joint_names = node.joint_names
node.destroy_node()
'''
if joint_names:
while True:
try:
joint_velocity_limits = {name: float(input(f"Enter the velocity limit for joint '{name}' (or press Enter to skip): ").strip())} for name in joint_names}
break
except ValueError:
print("Invalid input. Please enter numeric values or leave blank to skip.")
'''
joint_velocity_limits = None
robot = None
cost_mask = None
break
print("Invalid input. Please enter 'y' or 'n'.")
node = OSC_ROS2_interface(joint_names, joint_velocity_limits, robot, cost_mask)
# Run ROS 2 spin, and osc_process will be handled by the timer
try:
rclpy.spin(node)
except KeyboardInterrupt:
print("")
finally:
csv_file = './pose_log.csv'
with open(csv_file, 'w', newline='') as f:
writer = csv.writer(f)
writer.writerow(["timestamp", "x", "y", "z", "roll", "pitch", "yaw"])
with open(csv_file, 'a', newline='') as f:
writer = csv.writer(f)
for command in node.commands:
writer.writerow(command)
node.destroy_node()
rclpy.shutdown()
osc_terminate()
if __name__ == '__main__':
main()

78
workspace/src/osc_ros2/osc_ros2/osc_ros2_unit_quater.py → workspace/src/osc_ros2/osc_ros2/osc_ros2_rpy.py
View File

@@ -369,6 +369,7 @@ class OSC_ROS2_interface(Node):
def tcp_coordinates_handler(self, *args):
# Ensure the desired joint positions are within the specified limits
print("tcp_coordinates_handler")
if self.robot:
try:
if len(args) == 6:
@@ -453,39 +454,38 @@ class OSC_ROS2_interface(Node):
def send_joint_positions(self):
pass
def trapezoidal_timestamps(self, n_points, total_duration, accel_fraction=0.2):
def trapezoidal_timestamps(self, num_points, total_duration, flat_ratio):
"""
Generate timestamps for a trapezoidal velocity profile.
Generate symmetric timestamps with increasing → decreasing → flat → increasing spacing pattern.
Parameters:
n_points (int): Total number of trajectory points.
total_duration (float): Duration of the full motion [seconds].
accel_fraction (float): Fraction of the total time spent accelerating and decelerating.
Default is 0.2 (i.e., 20% accel, 60% cruise, 20% decel)
Args:
total_duration (float): The total duration (last timestamp).
num_points (int): Total number of timestamps (must be >= 3).
flat_ratio (float): Fraction of timestamps in the constant-spacing center (0.0–0.9).
Returns:
list of float: Timestamps for each point [seconds].
List[float]: List of timestamps from 0 to total_duration.
"""
if num_points < 3:
raise ValueError("Need at least 3 points for symmetry.")
if n_points == 2:
return [0.0, total_duration]
# Calculate how many are in the flat middle section
flat_count = int(num_points * flat_ratio)
if flat_count % 2 == 0:
flat_count += 1 # ensure symmetry
edge_count = (num_points - flat_count) // 2
# Time fractions
ta = accel_fraction * total_duration # acceleration time
tc = total_duration - 2 * ta # constant velocity time
# Create increasing durations on the sides
edge = np.linspace(1.5, 0.5, edge_count)
durations = np.concatenate([edge, np.full(flat_count, 0.5), edge[::-1]])
# Number of points per segment
n_accel = int(n_points * accel_fraction)
if n_accel == 0:
return list(np.linspace(0, total_duration, n_points))
n_cruise = n_points - 2*n_accel
# Time vectors for each segment
t_accel = np.linspace(0, ta, n_accel, endpoint=False)
t_cruise = np.linspace(ta, ta + tc, n_cruise, endpoint=False)
t_decel = np.linspace(ta + tc, total_duration, n_accel, endpoint=True)
# Normalize durations so they sum to total_duration
durations *= total_duration / durations.sum()
timestamps = np.concatenate([t_accel, t_cruise, t_decel])
return list(timestamps)
# Convert to timestamps
timestamps = np.cumsum(np.insert(durations, 0, 0.0))
return timestamps.tolist()
@@ -495,29 +495,18 @@ class OSC_ROS2_interface(Node):
msg = JointTrajectory()
msg.joint_names = self.joint_names
steps_per_m = 100
[x, y, z] = self.robot.fkine(self.current_joint_positions).t
q0 = sm.UnitQuaternion(self.robot.fkine(self.current_joint_positions).R)
[x,y,z] = self.robot.fkine(self.current_joint_positions).t
[roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7]
q1 = sm.UnitQuaternion.RPY(roll1, pitch1, yaw1)
steps = int(np.linalg.norm(np.array([x1, y1, z1]) - self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
if steps < 2:
steps = 2
cart_traj = [
sm.SE3([
x + (x1 - x) / (steps - 1) * i,
y + (y1 - y) / (steps - 1) * i,
z + (z1 - z) / (steps - 1) * i
]) * q0.interp(q1, i / (steps - 1)).SE3()
for i in range(steps)
]
print(cart_traj)
#self.prev_pose = self.desired[1:]
steps = int(np.linalg.norm(np.array([x1, y1, z1])- self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
if steps < 2: steps = 2
cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)]
if self.desired[-1]:
timestamps = self.trapezoidal_timestamps(steps, self.desired[-1])
timestamps = self.trapezoidal_timestamps(steps, self.desired[-1], 0.3)
for j in range(steps):
sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
if sol[1] == 1:
@@ -756,7 +745,6 @@ def main():
break
print("Invalid input. Please enter 'y' or 'n'.")
node = OSC_ROS2_interface(joint_names, joint_velocity_limits, robot, cost_mask)
# Run ROS 2 spin, and osc_process will be handled by the timer

2
workspace/src/osc_ros2/setup.py
View File

@@ -28,7 +28,7 @@ setup(
entry_points={
'console_scripts': [
'interface = osc_ros2.osc_ros2:main',
'interface1 = osc_ros2.osc_ros2_unit_quater:main',
'interface1 = osc_ros2.osc_ros2_rpy:main',
],
},
)