AS: cleaup
This commit is contained in:
Alexander Schaefer
@@ -1,38 +0,0 @@
|
||||
from osc4py3.as_eventloop import *
|
||||
from osc4py3 import oscmethod as osm
|
||||
import time
|
||||
|
||||
def joint_states_handler(*args):
|
||||
"""Handler function to process incoming joint states."""
|
||||
print(args)
|
||||
|
||||
def main():
|
||||
ip = "0.0.0.0" # IP address to listen on
|
||||
port = 8000 # Port to listen on
|
||||
|
||||
# Start the OSC system
|
||||
osc_startup()
|
||||
|
||||
# Make server channels to receive packets
|
||||
osc_udp_server(ip, port, "osc_server")
|
||||
|
||||
# Associate Python functions with message address patterns
|
||||
osc_method("/tcp_position_t", joint_states_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
|
||||
print(f"Listening for OSC messages on {ip}:{port}...")
|
||||
|
||||
try:
|
||||
# Run the event loop
|
||||
while True:
|
||||
osc_process() # Process OSC messages
|
||||
time.sleep(0.01) # Sleep to avoid high CPU usage
|
||||
|
||||
except KeyboardInterrupt:
|
||||
print("")
|
||||
|
||||
finally:
|
||||
# Properly close the system
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,18 +0,0 @@
|
||||
import roboticstoolbox as rtb
|
||||
import spatialmath as sm
|
||||
robot = rtb.ERobot.URDF('/BA/workspace/src/painting_robot_control/painting_robot_control/painting_robot.urdf')
|
||||
robot1 = rtb.ERobot.URDF('/BA/ur10e.urdf')
|
||||
"""print(robot)
|
||||
print(T)
|
||||
sol = robot.ik_LM(T)
|
||||
print(sol[1])"""
|
||||
print(robot1.n)
|
||||
|
||||
'''T = sm.SE3(0.2,0.2,0.0)*sm.SE3.Rz(0.5)
|
||||
#T = robot.fkine([-2,-0.5])
|
||||
|
||||
print(T)
|
||||
sol = robot.ik_LM(T, mask = [1,0,0,0,0,1], joint_limits = False)
|
||||
print(sol[1])
|
||||
print(sol[0])
|
||||
print(robot.fkine(sol[0]))'''
|
||||
@@ -1,53 +0,0 @@
|
||||
from osc4py3.as_eventloop import *
|
||||
from osc4py3 import oscbuildparse
|
||||
import time
|
||||
import roboticstoolbox as rtb
|
||||
|
||||
def main():
|
||||
# Start the OSC system
|
||||
osc_startup()
|
||||
|
||||
# Make client channels to send packets
|
||||
|
||||
#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.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)
|
||||
msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions2)
|
||||
osc_send(msg, "osc_client")
|
||||
osc_process()
|
||||
print("Sent joint positions2")
|
||||
time.sleep(3)
|
||||
msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions3)
|
||||
osc_send(msg, "osc_client")
|
||||
osc_process()
|
||||
print("Sent joint positions3")
|
||||
time.sleep(3)
|
||||
msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions4)
|
||||
osc_send(msg, "osc_client")
|
||||
osc_process()
|
||||
print("Sent joint positions4")
|
||||
time.sleep(3)
|
||||
msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions5)
|
||||
osc_send(msg, "osc_client")
|
||||
osc_process()
|
||||
print("Sent joint positions5")
|
||||
time.sleep(3)
|
||||
'''
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,108 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from osc4py3.as_eventloop import *
|
||||
from osc4py3 import oscmethod as osm
|
||||
import roboticstoolbox as rtb
|
||||
import spatialmath as sm
|
||||
import xml.etree.ElementTree as ET
|
||||
import time
|
||||
|
||||
class ScaledJointTrajectoryPublisher(Node):
|
||||
"""Node to publish joint trajectories based on OSC messages."""
|
||||
def __init__(self, robot, joint_names):
|
||||
super().__init__('scaled_joint_trajectory_publisher')
|
||||
|
||||
# ROS2 Publisher
|
||||
self.publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
'/scaled_joint_trajectory_controller/joint_trajectory',
|
||||
10
|
||||
)
|
||||
|
||||
# Store received joint positions
|
||||
self.joint_positions = [0.0] * len(joint_names)
|
||||
self.joint_names = joint_names
|
||||
self.robot = robot
|
||||
|
||||
osc_startup()
|
||||
osc_udp_server("0.0.0.0", 8000, "osc_server")
|
||||
print("Server started on 0.0.0.0:8000 \nready to receive messages in the following format: /tcp_coordinates [x, y, z, roll, pitch, yaw] optional: duration as last argument: /tcp_coordinates [x, y, z, roll, pitch, yaw, duration]")
|
||||
# Register OSC handler
|
||||
osc_method("/tcp_coordinates", self.tcp_coordinates_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
|
||||
def tcp_coordinates_handler(self, *args):
|
||||
"""Handles incoming OSC messages for tcp position."""
|
||||
time1 = time.time()
|
||||
if len(args) == len(self.joint_positions):
|
||||
x, y, z, roll, pitch, yaw = args
|
||||
duration = 4.0 # Default duration
|
||||
elif len(args) == len(self.joint_positions) + 1:
|
||||
x, y, z, roll, pitch, yaw, duration = args
|
||||
else:
|
||||
print("Invalid number of arguments")
|
||||
return
|
||||
|
||||
# Create the desired end-effector pose
|
||||
Tep = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
|
||||
|
||||
# Compute the inverse kinematics to get the joint angles
|
||||
#time1 = time.time()
|
||||
#sol = self.robot.ikine_LM(Tep)
|
||||
#print(f"Time taken for ERobot: {time.time() - time1}")
|
||||
#time1 = time.time()
|
||||
#sol = self.robot.ikine_LM(Tep, q0=self.joint_positions)
|
||||
#print(f"Time taken for ERobot with initial guess: {time.time() - time1}")
|
||||
#time1 = time.time()
|
||||
#sol = self.robot.ets().ikine_LM(Tep)
|
||||
#print(f"Time taken for ETS: {time.time() - time1}")
|
||||
#time1 = time.time()
|
||||
sol = self.robot.ik_LM(Tep, q0=self.joint_positions)
|
||||
#print(f"Time taken for ETS with initial guess: {time.time() - time1}")
|
||||
if sol[1]:
|
||||
joint_positions = list(sol[0])
|
||||
self.send_trajectory(joint_positions, duration)
|
||||
#print(f"Computed joint positions: {joint_positions}")
|
||||
else:
|
||||
print("Inverse kinematics failed")
|
||||
print(f"Frequency: {1/(time.time() - time1)} Hz")
|
||||
|
||||
def send_trajectory(self, joint_positions, duration=4.0):
|
||||
"""Publish a joint trajectory command to move the robot."""
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = joint_positions # Updated joint positions
|
||||
point.time_from_start.sec = int(duration)
|
||||
point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
|
||||
|
||||
msg.points.append(point)
|
||||
self.publisher.publish(msg)
|
||||
print(f"Updated joint positions: {joint_positions}")
|
||||
|
||||
def main():
|
||||
"""Main function to get joint names and start the ROS 2 & OSC system."""
|
||||
|
||||
tree = ET.parse('/BA/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('/BA/robot.urdf')
|
||||
|
||||
rclpy.init()
|
||||
|
||||
node = ScaledJointTrajectoryPublisher(robot.ets(), joint_names)
|
||||
|
||||
# Run both ROS 2 and OSC Server together
|
||||
try:
|
||||
while rclpy.ok():
|
||||
osc_process() # Handle one OSC request at a time
|
||||
rclpy.spin_once(node, timeout_sec=0.1) # Process ROS callbacks
|
||||
except KeyboardInterrupt:
|
||||
print("")
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,83 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from osc4py3.as_eventloop import *
|
||||
from osc4py3 import oscmethod as osm
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
class ScaledJointTrajectoryPublisher(Node):
|
||||
"""Node to publish joint trajectories based on OSC messages."""
|
||||
def __init__(self, joint_names):
|
||||
super().__init__('scaled_joint_trajectory_publisher')
|
||||
|
||||
# ROS2 Publisher
|
||||
self.publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
'/scaled_joint_trajectory_controller/joint_trajectory',
|
||||
10
|
||||
)
|
||||
|
||||
# Store received joint positions
|
||||
self.joint_positions = [0.0] * len(joint_names)
|
||||
self.joint_names = joint_names
|
||||
|
||||
osc_startup()
|
||||
osc_udp_server("0.0.0.0", 8000, "osc_server")
|
||||
print("Server started on 0.0.0.0:8000 \nready to receive messages in the following format: \n /joint_angles [joint_positions]; optional: duration as last element, default is 3sec")
|
||||
# Register OSC handler
|
||||
osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
|
||||
def joint_angles_handler(self, *args):
|
||||
"""Handles incoming OSC messages for joint positions."""
|
||||
print(args)
|
||||
if len(args) == len(self.joint_positions):
|
||||
self.joint_positions = list(args)
|
||||
print(self.joint_positions)
|
||||
self.send_trajectory(self.joint_positions)
|
||||
elif len(args) == len(self.joint_positions) + 1:
|
||||
self.joint_positions = args[:-1]
|
||||
self.send_trajectory(self.joint_positions, args[-1])
|
||||
print(f'Duration: {args[-1]}')
|
||||
|
||||
|
||||
|
||||
def send_trajectory(self, joint_positions, duration=0.01):
|
||||
"""Publish a joint trajectory command to move the robot."""
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
point = JointTrajectoryPoint()
|
||||
joint_positions = [float(joint) for joint in joint_positions]
|
||||
point.positions = joint_positions # Updated joint positions
|
||||
point.time_from_start.sec = int(duration)
|
||||
point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
|
||||
|
||||
msg.points.append(point)
|
||||
self.publisher.publish(msg)
|
||||
print(f"Updated joint positions: {self.joint_positions}")
|
||||
|
||||
|
||||
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']
|
||||
|
||||
rclpy.init()
|
||||
|
||||
node = ScaledJointTrajectoryPublisher(joint_names)
|
||||
|
||||
# Run both ROS 2 and OSC Server together
|
||||
try:
|
||||
while rclpy.ok():
|
||||
osc_process() # Handle one OSC request at a time
|
||||
rclpy.spin_once(node, timeout_sec=0.1) # Process ROS callbacks
|
||||
except KeyboardInterrupt:
|
||||
print("")
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,209 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from sensor_msgs.msg import JointState
|
||||
from osc4py3.as_allthreads import *
|
||||
from osc4py3 import oscmethod as osm
|
||||
import xml.etree.ElementTree as ET
|
||||
import time
|
||||
import numpy as np
|
||||
import os
|
||||
|
||||
class ScaledJointTrajectoryPublisher(Node):
|
||||
"""Node to publish joint trajectories based on OSC messages."""
|
||||
|
||||
def __init__(self, joint_names, joint_velocity_limits):
|
||||
super().__init__('scaled_joint_trajectory_publisher')
|
||||
|
||||
# ROS2 Publisher
|
||||
self.publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
'/scaled_joint_trajectory_controller/joint_trajectory',
|
||||
1
|
||||
)
|
||||
|
||||
self.subscription = self.create_subscription(
|
||||
JointState,
|
||||
'/joint_states',
|
||||
self.joint_states_callback,
|
||||
1 # Increased queue size for joint states
|
||||
)
|
||||
|
||||
# Store received joint positions
|
||||
self.current_joint_positions = [0.0] * len(joint_names)
|
||||
self.joint_names = joint_names
|
||||
self.joint_velocity_limits = joint_velocity_limits
|
||||
self.desired_joint_positions = [0.0] * len(joint_names)
|
||||
self.previous_desired = [0.0] * len(joint_names)
|
||||
|
||||
ip = "0.0.0.0" # Listen on all network interfaces
|
||||
port = 8000 # Must match the sender's port in `joint_state_osc.py`
|
||||
|
||||
osc_startup()
|
||||
osc_udp_server(ip, port, "osc_server")
|
||||
print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [joint_positions] optional: duration as last argument")
|
||||
# Register OSC handler
|
||||
osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
|
||||
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}")
|
||||
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.")
|
||||
else:
|
||||
print("Invalid input. Please enter 'y' or 'n'.")
|
||||
except ValueError:
|
||||
print("Invalid input. Please enter numeric values only.")
|
||||
|
||||
# Ask the user if they want to set new joint limits
|
||||
update_limits = input("Do you want to set new joint limits? (y/n): ").strip().lower()
|
||||
if update_limits == 'y':
|
||||
for joint_name in self.joint_names:
|
||||
while True:
|
||||
try:
|
||||
print(f"Current position limits for joint '{joint_name}': {self.robot.links[joint_name]} rad")
|
||||
lower_limit = input(f"Enter the new lower limit for joint '{joint_name}' (or press Enter to keep current): ").strip()
|
||||
upper_limit = input(f"Enter the new upper limit for joint '{joint_name}' (or press Enter to keep current): ").strip()
|
||||
|
||||
if lower_limit is not None and upper_limit is not None and lower_limit >= upper_limit:
|
||||
print("Invalid input. Lower limit must be less than upper limit.")
|
||||
continue
|
||||
|
||||
if lower_limit:
|
||||
self.robot.links[joint_name][0] = float(lower_limit)
|
||||
if upper_limit:
|
||||
self.robot.links[joint_name][1] = float(upper_limit)
|
||||
break
|
||||
except ValueError:
|
||||
print("Invalid input. Please enter numeric values or leave blank to keep current limits.")
|
||||
|
||||
|
||||
self.hz = float(input("Enter the desired refresh frequency (Hz): "))
|
||||
# Start the OSC server in a separate thread to avoid blocking the ROS 2 event loop
|
||||
self.create_timer(1/self.hz, self.update_position) # Timer to call osc_process periodically
|
||||
|
||||
def joint_angles_handler(self, *args):
|
||||
# Ensure the desired joint positions are within the specified limits
|
||||
|
||||
x, y, z, r, p, yaw, *_ = [float(i) for i in list(args)]
|
||||
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"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_joint_positions = [x, y, z, r, p, yaw]
|
||||
|
||||
|
||||
def joint_states_callback(self, msg):
|
||||
"""Callback function to handle incoming joint states."""
|
||||
joint_names = msg.name # List of joint names
|
||||
joint_position_dict = dict(zip(joint_names, self.current_joint_positions))
|
||||
self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
|
||||
|
||||
def update_position(self):
|
||||
time1 = time.time()
|
||||
if self.desired_joint_positions == self.previous_desired:
|
||||
return
|
||||
duration = 0
|
||||
for p1, p2, max_vel in zip(self.desired_joint_positions, self.current_joint_positions, self.joint_velocity_limits.values()):
|
||||
duration = max(max(duration, abs(p1 - p2) / max_vel),1/self.hz)# as minimun
|
||||
duration *= 1.8
|
||||
#print(f"Duration: {duration}")
|
||||
#print(f'vel: {max(np.array(self.desired_joint_positions_joint_positions) - np.array(self.joint_positions)/duration)}')
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = self.desired_joint_positions # Updated joint positions
|
||||
point.time_from_start.sec = int(duration)
|
||||
#point.time_from_start.sec = 10
|
||||
point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
|
||||
#point.time_from_start.nanosec = 0
|
||||
#point.velocities = [i*0.95 for i in self.joint_velocity_limits.values()]
|
||||
msg.points.append(point)
|
||||
self.publisher.publish(msg)
|
||||
#print(f"desired: {self.desired_joint_positions}")
|
||||
#print(f"desired: {[180/3.141*i for i in self.desired_joint_positions]}")
|
||||
#print(f"current: {[180/3.141*i for i in self.current_joint_positions]}")
|
||||
self.previous_desired = self.desired_joint_positions
|
||||
|
||||
def main():
|
||||
"""Main function to get joint names and start the ROS 2 & OSC system."""
|
||||
while True:
|
||||
path_to_urdf = input("Enter the path to the URDF file: ")
|
||||
if os.path.isfile(path_to_urdf):
|
||||
if not path_to_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(path_to_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_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)
|
||||
|
||||
rclpy.init()
|
||||
|
||||
node = ScaledJointTrajectoryPublisher(joint_names, joint_velocity_limits)
|
||||
|
||||
# Run ROS 2 spin, and osc_process will be handled by the timer
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
print("")
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,158 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from sensor_msgs.msg import JointState
|
||||
from osc4py3.as_allthreads import *
|
||||
from osc4py3 import oscmethod as osm
|
||||
import xml.etree.ElementTree as ET
|
||||
import time
|
||||
import numpy as np
|
||||
import spatialmath as sm
|
||||
import roboticstoolbox as rtb
|
||||
import os
|
||||
|
||||
class ScaledJointTrajectoryPublisher(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')
|
||||
|
||||
# ROS2 Publisher
|
||||
self.publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
'/scaled_joint_trajectory_controller/joint_trajectory',
|
||||
1
|
||||
)
|
||||
|
||||
self.subscription = self.create_subscription(
|
||||
JointState,
|
||||
'/joint_states',
|
||||
self.joint_states_callback,
|
||||
1 # Increased queue size for joint states
|
||||
)
|
||||
|
||||
# Store received joint positions
|
||||
self.current_joint_positions = [0.0] * len(joint_names)
|
||||
self.joint_names = joint_names
|
||||
self.joint_velocity_limits = joint_velocity_limits
|
||||
self.desired_joint_positions = [0.0] * len(joint_names)
|
||||
self.previous_desired = [0.0] * len(joint_names)
|
||||
self.robot = robot
|
||||
self.cost_mask = cost_mask
|
||||
|
||||
ip = "0.0.0.0" # Listen on all network interfaces
|
||||
port = 8000 # Must match the sender's port in `joint_state_osc.py`
|
||||
|
||||
osc_startup()
|
||||
osc_udp_server(ip, port, "osc_server")
|
||||
print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [joint_positions] optional: duration as last argument")
|
||||
# Register OSC handler
|
||||
osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
|
||||
|
||||
self.hz = float(input("Enter the desired refresh frequency (Hz): "))
|
||||
# Start the OSC server in a separate thread to avoid blocking the ROS 2 event loop
|
||||
self.create_timer(1/self.hz, self.update_position) # Timer to call osc_process periodically
|
||||
|
||||
def joint_angles_handler(self, *args):
|
||||
"""Handles incoming OSC messages for joint positions."""
|
||||
self.desired_joint_positions = [float(i) for i in list(args)]
|
||||
|
||||
|
||||
def joint_states_callback(self, msg):
|
||||
"""Callback function to handle incoming joint states."""
|
||||
joint_position_dict = dict(zip(msg.name, msg.position))
|
||||
self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
|
||||
|
||||
|
||||
def update_position(self):
|
||||
if self.desired_joint_positions == self.previous_desired:
|
||||
return
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
steps_per_m = 30
|
||||
if True: #len(args[0]) == len(self.joint_names):
|
||||
prev_duration = 0
|
||||
T1 = self.robot.fkine(self.current_joint_positions)
|
||||
[x,y,z] = T1.t
|
||||
[roll, pitch, yaw] = T1.rpy()
|
||||
x1, y1, z1, roll1, pitch1, yaw1 = self.desired_joint_positions
|
||||
steps = int(np.linalg.norm(np.array([x1-x, y1-y, z1-z])) * 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)]
|
||||
for j in range(steps):
|
||||
print(cart_traj[j])
|
||||
sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True)
|
||||
if sol[1] == 1:
|
||||
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
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = list(sol[0])
|
||||
#duration *= 2
|
||||
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)
|
||||
prev_sol = list(sol[0])
|
||||
else:
|
||||
print('IK could not find a solution!')
|
||||
prev_sol = self.current_joint_positions
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
self.publisher.publish(msg)
|
||||
self.previous_desired = self.desired_joint_positions
|
||||
|
||||
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}")
|
||||
|
||||
rclpy.init()
|
||||
|
||||
node = ScaledJointTrajectoryPublisher(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:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,272 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from sensor_msgs.msg import JointState
|
||||
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
|
||||
|
||||
class ScaledJointTrajectoryPublisher(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')
|
||||
|
||||
# ROS2 Publisher
|
||||
self.publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
'/scaled_joint_trajectory_controller/joint_trajectory',
|
||||
1
|
||||
)
|
||||
|
||||
self.subscription = self.create_subscription(
|
||||
JointState,
|
||||
'/joint_states',
|
||||
self.joint_states_callback,
|
||||
1 # Increased queue size for joint states
|
||||
)
|
||||
|
||||
# Store received joint positions
|
||||
self.current_joint_positions = [0.0] * len(joint_names)
|
||||
self.joint_names = joint_names
|
||||
self.joint_velocity_limits = joint_velocity_limits
|
||||
self.desired_joint_positions = [0.0] * len(joint_names)
|
||||
self.previous_desired = [0.0] * len(joint_names)
|
||||
self.robot = robot
|
||||
self.cost_mask = cost_mask
|
||||
self.prev_pose = None
|
||||
|
||||
ip = "0.0.0.0" # Listen on all network interfaces
|
||||
port = 8000 # Must match the sender's port in `joint_state_osc.py`
|
||||
|
||||
osc_startup()
|
||||
osc_udp_server(ip, port, "osc_server")
|
||||
print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [joint_positions] optional: duration as last argument")
|
||||
# Register OSC handler
|
||||
osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
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.")
|
||||
|
||||
# Ask the user if they want to set new joint limits
|
||||
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(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:
|
||||
lim[0][i] = float(lower_limit)
|
||||
if upper_limit:
|
||||
lim[1][i] = float(upper_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.")
|
||||
'''
|
||||
use_link_mask = input("Do you want to use a link mask? (y/n): ").strip().lower()
|
||||
if use_link_mask == 'y':
|
||||
while True:
|
||||
try:
|
||||
'''
|
||||
|
||||
|
||||
self.hz = float(input("Enter the desired refresh frequency (Hz): "))
|
||||
# Start the OSC server in a separate thread to avoid blocking the ROS 2 event loop
|
||||
self.create_timer(1/self.hz, self.update_position) # Timer to call osc_process periodically
|
||||
|
||||
def joint_angles_handler(self, *args):
|
||||
# Ensure the desired joint positions are within the specified limits
|
||||
#print("received joint angles")
|
||||
|
||||
x, y, z, r, p, yaw, *_ = [float(i) for i in list(args)]
|
||||
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"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_joint_positions = [x, y, z, r, p, yaw]
|
||||
|
||||
|
||||
def joint_states_callback(self, msg):
|
||||
"""Callback function to handle incoming joint states."""
|
||||
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]
|
||||
|
||||
def update_position(self):
|
||||
if self.desired_joint_positions == self.previous_desired:
|
||||
return
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
steps_per_m = 100
|
||||
if True: #len(args[0]) == len(self.joint_names):
|
||||
prev_duration = 0
|
||||
if self.prev_pose == 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:]
|
||||
x1, y1, z1, roll1, pitch1, yaw1 = self.desired_joint_positions
|
||||
self.prev_pose = self.desired_joint_positions
|
||||
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)]
|
||||
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("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 = self.desired_joint_positions
|
||||
'''
|
||||
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 *= 5
|
||||
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:
|
||||
print(f'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:
|
||||
return
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
self.publisher.publish(msg)
|
||||
self.previous_desired = self.desired_joint_positions
|
||||
|
||||
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)
|
||||
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("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}")
|
||||
|
||||
rclpy.init()
|
||||
|
||||
node = ScaledJointTrajectoryPublisher(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:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,232 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from sensor_msgs.msg import JointState
|
||||
from osc4py3.as_allthreads import *
|
||||
from osc4py3 import oscmethod as osm
|
||||
import xml.etree.ElementTree as ET
|
||||
import time
|
||||
import numpy as np
|
||||
import spatialmath as sm
|
||||
import roboticstoolbox as rtb
|
||||
import os
|
||||
|
||||
class ScaledJointTrajectoryPublisher(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')
|
||||
|
||||
# ROS2 Publisher
|
||||
self.publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
'/scaled_joint_trajectory_controller/joint_trajectory',
|
||||
1
|
||||
)
|
||||
|
||||
self.subscription = self.create_subscription(
|
||||
JointState,
|
||||
'/joint_states',
|
||||
self.joint_states_callback,
|
||||
1 # Increased queue size for joint states
|
||||
)
|
||||
|
||||
# Store received joint positions
|
||||
self.current_joint_positions = [0.0] * len(joint_names)
|
||||
self.joint_names = joint_names
|
||||
self.joint_velocity_limits = joint_velocity_limits
|
||||
self.desired_joint_positions = [0.0] * len(joint_names)
|
||||
self.previous_desired = [0.0] * len(joint_names)
|
||||
self.robot = robot
|
||||
self.cost_mask = cost_mask
|
||||
|
||||
ip = "0.0.0.0" # Listen on all network interfaces
|
||||
port = 8000 # Must match the sender's port in `joint_state_osc.py`
|
||||
|
||||
osc_startup()
|
||||
osc_udp_server(ip, port, "osc_server")
|
||||
print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [joint_positions] optional: duration as last argument")
|
||||
# Register OSC handler
|
||||
osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
|
||||
|
||||
self.hz = float(input("Enter the desired refresh frequency (Hz): "))
|
||||
# Start the OSC server in a separate thread to avoid blocking the ROS 2 event loop
|
||||
self.create_timer(1/self.hz, self.update_position) # Timer to call osc_process periodically
|
||||
|
||||
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}")
|
||||
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.")
|
||||
else:
|
||||
print("Invalid input. Please enter 'y' or 'n'.")
|
||||
except ValueError:
|
||||
print("Invalid input. Please enter numeric values only.")
|
||||
|
||||
def joint_angles_handler(self, *args):
|
||||
# Ensure the desired joint positions are within the specified limits
|
||||
|
||||
x, y, z, r, p, yaw, *_ = [float(i) for i in list(args)]
|
||||
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"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_joint_positions = [x, y, z, r, p, yaw]
|
||||
|
||||
|
||||
def joint_states_callback(self, msg):
|
||||
"""Callback function to handle incoming joint states."""
|
||||
joint_position_dict = dict(zip(msg.name, msg.position))
|
||||
self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
|
||||
|
||||
def rampfunction(self, startvalue, blendtime, currenttime):
|
||||
"""
|
||||
Ramp function to create a smooth transition from startvalue to 1 over blendtime seconds.
|
||||
"""
|
||||
if currenttime < blendtime:
|
||||
return startvalue + (1 - startvalue) * (currenttime / blendtime)
|
||||
else:
|
||||
return 1
|
||||
|
||||
def update_position(self):
|
||||
if self.desired_joint_positions == self.previous_desired:
|
||||
return
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
steps_per_m = 30
|
||||
if True: #len(args[0]) == len(self.joint_names):
|
||||
prev_duration = 0
|
||||
T1 = self.robot.fkine(self.current_joint_positions)
|
||||
[x,y,z] = T1.t
|
||||
[roll, pitch, yaw] = T1.rpy()
|
||||
x1, y1, z1, roll1, pitch1, yaw1 = self.desired_joint_positions
|
||||
steps = int(np.linalg.norm(np.array([x1-x, y1-y, z1-z])) * 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)]
|
||||
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)
|
||||
if sol[1] == 1:
|
||||
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
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = list(sol[0])
|
||||
duration /= self.rampfunction(0.1, 2, prev_duration)
|
||||
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)
|
||||
prev_sol = list(sol[0])
|
||||
else:
|
||||
print(f'IK could not find a solution for (x,y,z) = ({cart_traj[j].t}), (roll,pitch,yaw) = ({cart_traj[j].rpy()})!')
|
||||
prev_sol = self.current_joint_positions
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
self.publisher.publish(msg)
|
||||
self.previous_desired = self.desired_joint_positions
|
||||
|
||||
def main():
|
||||
"""Main function to get joint names and start the ROS 2 & OSC system."""
|
||||
while True:
|
||||
path_to_urdf = input("Enter the path to the URDF file: ")
|
||||
if os.path.isfile(path_to_urdf):
|
||||
if not path_to_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(path_to_urdf)
|
||||
root = tree.getroot()
|
||||
robot = rtb.ERobot.URDF(path_to_urdf)
|
||||
joint_names = [link.name for link in robot.links if link.isjoint]
|
||||
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)
|
||||
|
||||
print("The cost mask determines which coordinates are used for the IK. Each element of the cost mask corresponds to a Cartesian coordinate [x, y, z, roll, pitch, yaw].")
|
||||
print("The cost mask [1, 1, 1, 0, 0, 0] means that the IK will only consider translation and no rotation.")
|
||||
while True:
|
||||
try:
|
||||
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:
|
||||
considered_coords = [coord for coord, use in zip(['x', 'y', 'z', 'roll', 'pitch', 'yaw'], cost_mask) if use == 1]
|
||||
print(f"The following coordinates will be considered for IK: {', '.join(considered_coords)}")
|
||||
confirm = input("Are you sure you want to proceed with this cost mask? (y/n): ").strip().lower()
|
||||
if confirm == 'y':
|
||||
break
|
||||
elif confirm == 'n':
|
||||
print("Please re-enter the cost mask.")
|
||||
else:
|
||||
print("Invalid input. Please enter 'y' or 'n'.")
|
||||
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}")
|
||||
|
||||
rclpy.init()
|
||||
|
||||
node = ScaledJointTrajectoryPublisher(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:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,296 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from sensor_msgs.msg import JointState
|
||||
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
|
||||
|
||||
class ScaledJointTrajectoryPublisher(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')
|
||||
|
||||
# ROS2 Publisher
|
||||
self.publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
'/scaled_joint_trajectory_controller/joint_trajectory',
|
||||
1
|
||||
)
|
||||
|
||||
self.subscription = self.create_subscription(
|
||||
JointState,
|
||||
'/joint_states',
|
||||
self.joint_states_callback,
|
||||
1 # Increased queue size for joint states
|
||||
)
|
||||
|
||||
# Store received joint positions
|
||||
self.current_joint_positions = [0.0] * len(joint_names)
|
||||
self.joint_names = joint_names
|
||||
self.joint_velocity_limits = joint_velocity_limits
|
||||
self.desired_joint_positions = [0.0] * len(joint_names)
|
||||
self.previous_desired = [0.0] * len(joint_names)
|
||||
self.robot = robot
|
||||
self.cost_mask = cost_mask
|
||||
self.prev_pose = None
|
||||
|
||||
ip = "0.0.0.0" # Listen on all network interfaces
|
||||
port = 8000 # Must match the sender's port in `joint_state_osc.py`
|
||||
|
||||
osc_startup()
|
||||
osc_udp_server(ip, port, "osc_server")
|
||||
print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [joint_positions] optional: duration as last argument")
|
||||
# Register OSC handler
|
||||
osc_method("/coordinates", self.coordinates_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
|
||||
def joint_angles_handler(self, *args):
|
||||
"""Handles incoming OSC messages for joint positions."""
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
n=2
|
||||
for arg in args:
|
||||
if len(arg) == len(self.joint_names):
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = list(arg)
|
||||
point.time_from_start.sec = n
|
||||
n+=2
|
||||
point.time_from_start.nanosec = 0
|
||||
msg.points.append(point)
|
||||
elif len(arg) == len(self.joint_names) + 1:
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = list(arg[:-1])
|
||||
point.time_from_start.sec = int(arg[-1])
|
||||
point.time_from_start.nanosec = int((arg[-1] - int(arg[-1])) * 1e9)
|
||||
msg.points.append(point)
|
||||
|
||||
self.publisher.publish(msg)
|
||||
print("published joint positions")
|
||||
|
||||
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.")
|
||||
|
||||
# Ask the user if they want to set new joint limits
|
||||
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(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:
|
||||
lim[0][i] = float(lower_limit)
|
||||
if upper_limit:
|
||||
lim[1][i] = float(upper_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.")
|
||||
'''
|
||||
use_link_mask = input("Do you want to use a link mask? (y/n): ").strip().lower()
|
||||
if use_link_mask == 'y':
|
||||
while True:
|
||||
try:
|
||||
'''
|
||||
|
||||
|
||||
self.hz = float(input("Enter the desired refresh frequency (Hz): "))
|
||||
# Start the OSC server in a separate thread to avoid blocking the ROS 2 event loop
|
||||
self.create_timer(1/self.hz, self.update_position) # Timer to call osc_process periodically
|
||||
|
||||
def coordinates_handler(self, *args):
|
||||
# Ensure the desired joint positions are within the specified limits
|
||||
#print("received joint angles")
|
||||
|
||||
x, y, z, r, p, yaw, *_ = [float(i) for i in list(args)]
|
||||
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"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_joint_positions = [x, y, z, r, p, yaw]
|
||||
|
||||
|
||||
def joint_states_callback(self, msg):
|
||||
"""Callback function to handle incoming joint states."""
|
||||
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]
|
||||
|
||||
def update_position(self):
|
||||
if self.desired_joint_positions == self.previous_desired:
|
||||
return
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
steps_per_m = 100
|
||||
if True: #len(args[0]) == len(self.joint_names):
|
||||
prev_duration = 0
|
||||
if self.prev_pose == 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:]
|
||||
x1, y1, z1, roll1, pitch1, yaw1 = self.desired_joint_positions
|
||||
self.prev_pose = self.desired_joint_positions
|
||||
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)]
|
||||
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("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 = self.desired_joint_positions
|
||||
'''
|
||||
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 *= 5
|
||||
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:
|
||||
print(f'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:
|
||||
return
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
self.publisher.publish(msg)
|
||||
self.previous_desired = self.desired_joint_positions
|
||||
|
||||
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}")
|
||||
|
||||
rclpy.init()
|
||||
|
||||
node = ScaledJointTrajectoryPublisher(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:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,36 +0,0 @@
|
||||
import numpy as np
|
||||
from roboticstoolbox.tools.trajectory import mstraj
|
||||
|
||||
# Define via points (each row is a joint configuration)
|
||||
viapoints = np.array([
|
||||
[0, 0, 0], # Start
|
||||
[0.5, 0.2, -0.1], # Intermediate
|
||||
[1.0, 0.4, 0.2] # End
|
||||
])
|
||||
|
||||
# Time step
|
||||
dt = 0.01 # seconds
|
||||
|
||||
# Acceleration time
|
||||
tacc = 0.2 # seconds
|
||||
|
||||
# Maximum joint velocity per joint (same length as number of joints)
|
||||
qdmax = [0.5, 0.3, 0.4] # radians per second
|
||||
|
||||
# Optional: starting position (otherwise uses first viapoint)
|
||||
q0 = viapoints[0]
|
||||
|
||||
# Generate the trajectory
|
||||
traj = mstraj(
|
||||
viapoints=viapoints,
|
||||
dt=dt,
|
||||
tacc=tacc,
|
||||
qdmax=qdmax,
|
||||
)
|
||||
|
||||
# Extract trajectory
|
||||
time = traj.t # Time vector
|
||||
positions = traj.q # Joint angles (shape: K x N)
|
||||
|
||||
print("Time vector:", time)
|
||||
print("Joint positions:\n", positions)
|
||||
@@ -1,78 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from osc4py3.as_eventloop import *
|
||||
from osc4py3 import oscmethod as osm
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
class ScaledJointTrajectoryPublisher(Node):
|
||||
"""Node to publish joint trajectories based on OSC messages."""
|
||||
def __init__(self, joint_names):
|
||||
super().__init__('scaled_joint_trajectory_publisher')
|
||||
|
||||
# ROS2 Publisher
|
||||
self.publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
'/scaled_joint_trajectory_controller/joint_trajectory',
|
||||
10
|
||||
)
|
||||
|
||||
# Store received joint positions
|
||||
self.joint_positions = []
|
||||
self.joint_names = joint_names
|
||||
|
||||
osc_startup()
|
||||
osc_udp_server("0.0.0.0", 8000, "osc_server")
|
||||
print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [joint_positions] optional: duration as last argument")
|
||||
# Register OSC handler
|
||||
osc_method("/joint_angles", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
|
||||
def joint_angles_handler(self, *args):
|
||||
"""Handles incoming OSC messages for joint positions."""
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
n=2
|
||||
for arg in args:
|
||||
if len(arg) == len(self.joint_names):
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = list(arg)
|
||||
point.time_from_start.sec = n
|
||||
n+=2
|
||||
point.time_from_start.nanosec = 0
|
||||
msg.points.append(point)
|
||||
elif len(arg) == len(self.joint_names) + 1:
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = list(arg[:-1])
|
||||
point.time_from_start.sec = int(arg[-1])
|
||||
point.time_from_start.nanosec = int((arg[-1] - int(arg[-1])) * 1e9)
|
||||
msg.points.append(point)
|
||||
|
||||
self.publisher.publish(msg)
|
||||
print("published joint positions")
|
||||
|
||||
|
||||
def main():
|
||||
"""Main function to get joint names and start the ROS 2 & OSC system."""
|
||||
|
||||
tree = ET.parse('/BA/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']
|
||||
|
||||
rclpy.init()
|
||||
|
||||
node = ScaledJointTrajectoryPublisher(joint_names)
|
||||
|
||||
# Run both ROS 2 and OSC Server together
|
||||
try:
|
||||
while rclpy.ok():
|
||||
osc_process() # Handle one OSC request at a time
|
||||
rclpy.spin_once(node, timeout_sec=0.1) # Process ROS callbacks
|
||||
except KeyboardInterrupt:
|
||||
print("")
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,149 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from osc4py3.as_eventloop import *
|
||||
from osc4py3 import oscmethod as osm
|
||||
import xml.etree.ElementTree as ET
|
||||
import roboticstoolbox as rtb
|
||||
import spatialmath as sm
|
||||
import numpy as np
|
||||
import time
|
||||
|
||||
class ScaledJointTrajectoryPublisher(Node):
|
||||
"""Node to publish joint trajectories based on OSC messages."""
|
||||
def __init__(self, joint_names, robot, cost_mask):
|
||||
super().__init__('scaled_joint_trajectory_publisher')
|
||||
|
||||
self.cost_mask = cost_mask
|
||||
self.robot = robot
|
||||
self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
|
||||
|
||||
if self.trajectroy_topic_name == "":
|
||||
self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
|
||||
|
||||
# ROS2 Publisher
|
||||
self.publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
self.trajectroy_topic_name,
|
||||
10
|
||||
)
|
||||
|
||||
# Store received joint positions
|
||||
self.joint_names = joint_names
|
||||
|
||||
self.port = 8000 # UDP port
|
||||
|
||||
osc_startup()
|
||||
osc_udp_server("0.0.0.0", self.port, "osc_server")
|
||||
print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
|
||||
# Register OSC handler
|
||||
osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
|
||||
def joint_angles_handler(self, *args):
|
||||
"""Handles incoming OSC messages for joint positions."""
|
||||
time1 = time.time()
|
||||
print("Received joint positions")
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
joint_positions = [0.0] * len(self.joint_names)
|
||||
steps = 30
|
||||
vel = 0.4
|
||||
if True: #len(args[0]) == len(self.joint_names):
|
||||
n=2.0
|
||||
for i in range(len(args)-1):
|
||||
print(f'i = {i}')
|
||||
x, y, z, roll, pitch, yaw = args[i]
|
||||
print(1)
|
||||
Tep1 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
|
||||
print(2)
|
||||
x, y, z, roll, pitch, yaw = args[i+1]
|
||||
print(3)
|
||||
Tep2 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
|
||||
print(4)
|
||||
cart_traj = rtb.ctraj(Tep1, Tep2, steps)
|
||||
print(cart_traj)
|
||||
print(5)
|
||||
for j in range(steps):
|
||||
print(f'j = {j}')
|
||||
print(6)
|
||||
sol = self.robot.ik_LM(cart_traj[j], q0=joint_positions, mask = self.cost_mask, joint_limits = True)
|
||||
print(7)
|
||||
if sol[1] == 1:
|
||||
print(8)
|
||||
if j == 0: dist = vel*n
|
||||
else: dist = np.linalg.norm(cart_traj[j].t - cart_traj[j-1].t)
|
||||
print(9)
|
||||
point = JointTrajectoryPoint()
|
||||
print(10)
|
||||
point.positions = list(sol[0])
|
||||
print(11)
|
||||
joint_positions = list(sol[0])
|
||||
print(12)
|
||||
point.time_from_start.sec = int(n)
|
||||
print(13)
|
||||
point.time_from_start.nanosec = int((n - int(n)) * 1e9)
|
||||
print(14)
|
||||
n+=dist/vel
|
||||
print(16)
|
||||
msg.points.append(point)
|
||||
print(17)
|
||||
else: print('IK could not find a solution!')
|
||||
print(18)
|
||||
self.publisher.publish(msg)
|
||||
print(19)
|
||||
print(f"published joint positions {msg.points[-1]}")
|
||||
print(f'Frequency: {round(1/(time.time()-time1),2)} Hz')
|
||||
|
||||
'''
|
||||
elif len(args[0]) == len(self.joint_names) + 1:
|
||||
for i in range(len(args)):
|
||||
x, y, z, roll, pitch, yaw, timetag = args[i]
|
||||
Tep = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
|
||||
x, y, z, roll, pitch, yaw = args[i+1][:-1]
|
||||
Tep2 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
|
||||
cart_traj = rtb.ctraj(Tep, Tep2, steps)
|
||||
for Tep in cart_traj:
|
||||
sol = self.robot.ik_LM(Tep, q0=joint_positions)
|
||||
|
||||
else:
|
||||
print("Invalid number or format of arguments")'''
|
||||
|
||||
|
||||
|
||||
def main():
|
||||
"""Main function to get joint names and start the ROS 2 & OSC system."""
|
||||
path_to_urdf = input("Enter the path to the URDF file: ")
|
||||
tree = ET.parse(path_to_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(path_to_urdf)
|
||||
print(robot)
|
||||
rclpy.init()
|
||||
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) separated by spaces, of which <= {robot.n} are 1): ").split()]
|
||||
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}")
|
||||
node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask)
|
||||
|
||||
# Run both ROS 2 and OSC Server together
|
||||
try:
|
||||
while rclpy.ok():
|
||||
osc_process() # Handle one OSC request at a time
|
||||
rclpy.spin_once(node, timeout_sec=0.1) # Process ROS callbacks
|
||||
except KeyboardInterrupt:
|
||||
print("")
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,141 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from osc4py3.as_eventloop import *
|
||||
from osc4py3 import oscmethod as osm
|
||||
import xml.etree.ElementTree as ET
|
||||
import roboticstoolbox as rtb
|
||||
import spatialmath as sm
|
||||
import numpy as np
|
||||
import time
|
||||
import os
|
||||
|
||||
class ScaledJointTrajectoryPublisher(Node):
|
||||
"""Node to publish joint trajectories based on OSC messages."""
|
||||
def __init__(self, joint_names, robot, cost_mask, joint_velocity_limits):
|
||||
super().__init__('scaled_joint_trajectory_publisher')
|
||||
|
||||
self.joint_velocity_limits = joint_velocity_limits
|
||||
self.cost_mask = cost_mask
|
||||
self.robot = robot
|
||||
self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
|
||||
|
||||
if self.trajectroy_topic_name == "":
|
||||
self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
|
||||
|
||||
# ROS2 Publisher
|
||||
self.publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
self.trajectroy_topic_name,
|
||||
10
|
||||
)
|
||||
|
||||
# Store received joint positions
|
||||
self.joint_names = joint_names
|
||||
|
||||
self.port = 8000 # UDP port
|
||||
|
||||
osc_startup()
|
||||
osc_udp_server("0.0.0.0", self.port, "osc_server")
|
||||
print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
|
||||
# Register OSC handler
|
||||
osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
print("OSC method registered for /joint_trajectory")
|
||||
def joint_angles_handler(self, *args):
|
||||
"""Handles incoming OSC messages for joint positions."""
|
||||
print("Received joint positions")
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
steps = 50
|
||||
if True: #len(args[0]) == len(self.joint_names):
|
||||
prev_duration = 0
|
||||
for i in range(len(args)-1):
|
||||
x, y, z, roll, pitch, yaw = args[i]
|
||||
x1, y1, z1, roll1, pitch1, yaw1 = args[i+1]
|
||||
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], order='xyz') for i in range(steps)]
|
||||
prev_sol = [0.0,0.0,0.0,0.0,0.0,0.0] if i == 0 else sol[0]
|
||||
for j in (range(steps) if i == 0 else range(1,steps)):
|
||||
#print(cart_traj[j])
|
||||
sol = self.robot.ik_LM(cart_traj[j], q0=[0.0] * len(self.joint_names), mask = self.cost_mask, joint_limits = True) if i == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True)
|
||||
if sol[1] == 1:
|
||||
if list(sol[0])==list(prev_sol): continue
|
||||
duration = 0
|
||||
for p1, p2, max_vel in zip(sol[0], prev_sol, self.joint_velocity_limits.values()):
|
||||
duration = max(duration, abs(p1 - p2) / max_vel) # as minimun
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = list(sol[0])
|
||||
duration *= 1.6
|
||||
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)
|
||||
prev_sol = list(sol[0])
|
||||
else: print('IK could not find a solution!')
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
self.publisher.publish(msg)
|
||||
|
||||
def main():
|
||||
"""Main function to get joint names and start the ROS 2 & OSC system."""
|
||||
while True:
|
||||
path_to_urdf = input("Enter the path to the URDF file: ")
|
||||
if os.path.isfile(path_to_urdf):
|
||||
if not path_to_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(path_to_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(path_to_urdf)
|
||||
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)
|
||||
|
||||
|
||||
|
||||
rclpy.init()
|
||||
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}")
|
||||
node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask, joint_velocity_limits)
|
||||
|
||||
# Run both ROS 2 and OSC Server together
|
||||
try:
|
||||
while rclpy.ok():
|
||||
osc_process() # Handle one OSC request at a time
|
||||
rclpy.spin_once(node, timeout_sec=0.1) # Process ROS callbacks
|
||||
except KeyboardInterrupt:
|
||||
print("")
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,166 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from sensor_msgs.msg import JointState
|
||||
from osc4py3.as_allthreads import *
|
||||
from osc4py3 import oscmethod as osm
|
||||
import xml.etree.ElementTree as ET
|
||||
import roboticstoolbox as rtb
|
||||
import spatialmath as sm
|
||||
import numpy as np
|
||||
import time
|
||||
import os
|
||||
|
||||
class ScaledJointTrajectoryPublisher(Node):
|
||||
"""Node to publish joint trajectories based on OSC messages."""
|
||||
def __init__(self, joint_names, robot, cost_mask, joint_velocity_limits):
|
||||
super().__init__('scaled_joint_trajectory_publisher')
|
||||
|
||||
self.joint_velocity_limits = joint_velocity_limits
|
||||
self.cost_mask = cost_mask
|
||||
self.robot = robot
|
||||
self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
|
||||
|
||||
if self.trajectroy_topic_name == "":
|
||||
self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
|
||||
|
||||
# ROS2 Publisher
|
||||
self.publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
self.trajectroy_topic_name,
|
||||
10
|
||||
)
|
||||
self.subscription = self.create_subscription(
|
||||
JointState,
|
||||
'/joint_states',
|
||||
self.joint_states_callback,
|
||||
1 # Increased queue size for joint states
|
||||
)
|
||||
|
||||
|
||||
|
||||
|
||||
self.maximum_acceleration = [0.0] * len(joint_names)
|
||||
# Store received joint positions
|
||||
self.joint_names = joint_names
|
||||
for joint in joint_names:
|
||||
self.maximum_acceleration[joint_names.index(joint)] = float(input(f"Enter the maximum acceleration for joint {joint}: "))
|
||||
|
||||
self.port = 8000 # UDP port
|
||||
|
||||
osc_startup()
|
||||
osc_udp_server("0.0.0.0", self.port, "osc_server")
|
||||
print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
|
||||
# Register OSC handler
|
||||
osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
print("OSC method registered for /joint_trajectory")
|
||||
|
||||
def joint_states_callback(self, msg):
|
||||
"""Callback function to handle incoming joint states."""
|
||||
joint_position_dict = dict(zip(msg.name, msg.position))
|
||||
self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
|
||||
joint_velocity_dict = dict(zip(msg.name, msg.velocity))
|
||||
self.current_joint_velocities = [joint_velocity_dict[name] for name in self.joint_names]
|
||||
|
||||
def joint_angles_handler(self, *args):
|
||||
"""Handles incoming OSC messages for joint positions."""
|
||||
print("Received joint positions")
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
steps = 50
|
||||
if True: #len(args[0]) == len(self.joint_names):
|
||||
prev_duration = 0
|
||||
for i in range(len(args)-1):
|
||||
x, y, z, roll, pitch, yaw = args[i]
|
||||
x1, y1, z1, roll1, pitch1, yaw1 = args[i+1]
|
||||
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], order='xyz') for i in range(steps)]
|
||||
prev_sol = self.current_joint_positions if i == 0 else sol[0]
|
||||
for j in (range(steps) if i == 0 else range(1,steps)):
|
||||
#print(cart_traj[j])
|
||||
sol = self.robot.ik_LM(cart_traj[j], q0=[0.0] * len(self.joint_names), mask = self.cost_mask, joint_limits = True) if i == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True)
|
||||
if sol[1] == 1:
|
||||
if list(sol[0])==list(prev_sol): continue
|
||||
duration = 0
|
||||
for i, (p1, p2, max_vel) in enumerate(zip(sol[0], prev_sol, self.joint_velocity_limits.values())):
|
||||
print(f'joint {i}, p1: {p1}, p2: {p2}, max_vel: {max_vel}')
|
||||
if len(msg.points) == 0: v = self.current_joint_velocities[i]
|
||||
max_acc_duration = np.sqrt((v/self.maximum_acceleration[i])**2 + 2*(abs(p1 - p2)/self.maximum_acceleration[i]))- v/self.maximum_acceleration[i]
|
||||
duration = max(duration, abs(p1 - p2) / max_vel, max_acc_duration) # as minimun
|
||||
v = abs(p1 - p2) / duration
|
||||
print(f'duration: {duration}, max_acc_duration: {max_acc_duration}, max_vel_duration: { abs(p1 - p2) / max_vel}, v: {v}')
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = list(sol[0])
|
||||
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)
|
||||
prev_sol = list(sol[0])
|
||||
else: print('IK could not find a solution!')
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
self.publisher.publish(msg)
|
||||
|
||||
def main():
|
||||
"""Main function to get joint names and start the ROS 2 & OSC system."""
|
||||
while True:
|
||||
path_to_urdf = input("Enter the path to the URDF file: ")
|
||||
if os.path.isfile(path_to_urdf):
|
||||
if not path_to_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(path_to_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(path_to_urdf)
|
||||
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)
|
||||
|
||||
|
||||
|
||||
rclpy.init()
|
||||
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}")
|
||||
node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask, joint_velocity_limits)
|
||||
|
||||
# Run both ROS 2 and OSC Server together
|
||||
try:
|
||||
while rclpy.ok():
|
||||
osc_process() # Handle one OSC request at a time
|
||||
rclpy.spin_once(node, timeout_sec=0.1) # Process ROS callbacks
|
||||
except KeyboardInterrupt:
|
||||
print("")
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,152 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from osc4py3.as_eventloop import *
|
||||
from osc4py3 import oscmethod as osm
|
||||
import xml.etree.ElementTree as ET
|
||||
import roboticstoolbox as rtb
|
||||
import spatialmath as sm
|
||||
import numpy as np
|
||||
import time
|
||||
import os
|
||||
|
||||
class ScaledJointTrajectoryPublisher(Node):
|
||||
"""Node to publish joint trajectories based on OSC messages."""
|
||||
def __init__(self, joint_names, robot, cost_mask, joint_velocity_limits):
|
||||
super().__init__('scaled_joint_trajectory_publisher')
|
||||
|
||||
self.joint_velocity_limits = joint_velocity_limits
|
||||
self.cost_mask = cost_mask
|
||||
self.robot = robot
|
||||
self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
|
||||
|
||||
if self.trajectroy_topic_name == "":
|
||||
self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
|
||||
|
||||
# ROS2 Publisher
|
||||
self.publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
self.trajectroy_topic_name,
|
||||
10
|
||||
)
|
||||
|
||||
# Store received joint positions
|
||||
self.joint_names = joint_names
|
||||
|
||||
self.port = 8000 # UDP port
|
||||
|
||||
osc_startup()
|
||||
osc_udp_server("0.0.0.0", self.port, "osc_server")
|
||||
print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
|
||||
# Register OSC handler
|
||||
osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
print("OSC method registered for /joint_trajectory")
|
||||
|
||||
def rampfunction(self, startvalue, blendtime, currenttime):
|
||||
"""
|
||||
Ramp function to create a smooth transition from startvalue to 1 over blendtime seconds.
|
||||
"""
|
||||
if currenttime < blendtime:
|
||||
return startvalue + (1 - startvalue) * (currenttime / blendtime)
|
||||
else:
|
||||
return 1
|
||||
|
||||
|
||||
def joint_angles_handler(self, *args):
|
||||
"""Handles incoming OSC messages for joint positions."""
|
||||
print("Received joint positions")
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
steps = 50
|
||||
if True: #len(args[0]) == len(self.joint_names):
|
||||
prev_duration = 0
|
||||
for i in range(len(args)-1):
|
||||
x, y, z, roll, pitch, yaw = args[i]
|
||||
x1, y1, z1, roll1, pitch1, yaw1 = args[i+1]
|
||||
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], order='xyz') for i in range(steps)]
|
||||
prev_sol = [0.0,0.0,0.0,0.0,0.0,0.0] if i == 0 else sol[0]
|
||||
for j in (range(steps) if i == 0 else range(1,steps)):
|
||||
#print(cart_traj[j])
|
||||
sol = self.robot.ik_LM(cart_traj[j], q0=[0.0] * len(self.joint_names), mask = self.cost_mask, joint_limits = True) if i == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True)
|
||||
if sol[1] == 1:
|
||||
if list(sol[0])==list(prev_sol): continue
|
||||
duration = 0
|
||||
for p1, p2, max_vel in zip(sol[0], prev_sol, self.joint_velocity_limits.values()):
|
||||
duration = max(duration, abs(p1 - p2) / max_vel) # as minimun
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = list(sol[0])
|
||||
duration /= self.rampfunction(0.1, 2, prev_duration)
|
||||
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)
|
||||
prev_sol = list(sol[0])
|
||||
else: print('IK could not find a solution!')
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
self.publisher.publish(msg)
|
||||
|
||||
def main():
|
||||
"""Main function to get joint names and start the ROS 2 & OSC system."""
|
||||
while True:
|
||||
path_to_urdf = input("Enter the path to the URDF file: ")
|
||||
if os.path.isfile(path_to_urdf):
|
||||
if not path_to_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(path_to_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(path_to_urdf)
|
||||
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)
|
||||
|
||||
|
||||
|
||||
rclpy.init()
|
||||
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}")
|
||||
node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask, joint_velocity_limits)
|
||||
|
||||
# Run both ROS 2 and OSC Server together
|
||||
try:
|
||||
while rclpy.ok():
|
||||
osc_process() # Handle one OSC request at a time
|
||||
rclpy.spin_once(node, timeout_sec=0.1) # Process ROS callbacks
|
||||
except KeyboardInterrupt:
|
||||
print("")
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,166 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from sensor_msgs.msg import JointState
|
||||
from osc4py3.as_allthreads import *
|
||||
from osc4py3 import oscmethod as osm
|
||||
import xml.etree.ElementTree as ET
|
||||
import roboticstoolbox as rtb
|
||||
import spatialmath as sm
|
||||
import numpy as np
|
||||
import time
|
||||
import os
|
||||
|
||||
class ScaledJointTrajectoryPublisher(Node):
|
||||
"""Node to publish joint trajectories based on OSC messages."""
|
||||
def __init__(self, joint_names, robot, cost_mask, joint_velocity_limits):
|
||||
super().__init__('scaled_joint_trajectory_publisher')
|
||||
|
||||
self.joint_velocity_limits = [joint_velocity_limits[joint] for joint in joint_names]
|
||||
self.cost_mask = cost_mask
|
||||
self.robot = robot
|
||||
self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
|
||||
|
||||
if self.trajectroy_topic_name == "":
|
||||
self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
|
||||
|
||||
# ROS2 Publisher
|
||||
self.publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
self.trajectroy_topic_name,
|
||||
10
|
||||
)
|
||||
self.subscription = self.create_subscription(
|
||||
JointState,
|
||||
'/joint_states',
|
||||
self.joint_states_callback,
|
||||
1 # Increased queue size for joint states
|
||||
)
|
||||
# Store received joint positions
|
||||
self.joint_names = joint_names
|
||||
|
||||
self.port = 8000 # UDP port
|
||||
|
||||
osc_startup()
|
||||
osc_udp_server("0.0.0.0", self.port, "osc_server")
|
||||
print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
|
||||
# Register OSC handler
|
||||
osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
print("OSC method registered for /joint_trajectory")
|
||||
|
||||
def joint_states_callback(self, msg):
|
||||
"""Callback function to handle incoming joint states."""
|
||||
joint_position_dict = dict(zip(msg.name, msg.position))
|
||||
self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
|
||||
joint_velocity_dict = dict(zip(msg.name, msg.velocity))
|
||||
self.current_joint_velocities = [joint_velocity_dict[name] for name in self.joint_names]
|
||||
|
||||
|
||||
def joint_angles_handler(self, *args):
|
||||
"""Handles incoming OSC messages for joint positions."""
|
||||
try:
|
||||
print("Received joint positions")
|
||||
viapoints = []
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
steps_per_m = 4
|
||||
for i in range(len(args)-1):
|
||||
x, y, z, roll, pitch, yaw = args[i]
|
||||
x1, y1, z1, roll1, pitch1, yaw1 = args[i+1]
|
||||
steps = int(np.linalg.norm(np.array([x1-x, y1-y, z1-z])) * 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], order='xyz') for i in range(steps)]
|
||||
if i == 0: prev_sol = self.current_joint_positions
|
||||
for j in (range(steps) if i == 0 else range(1,steps)):
|
||||
#print(cart_traj[j])
|
||||
sol = self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True)
|
||||
if sol[1] == 1:
|
||||
viapoints.append(list(sol[0]))
|
||||
prev_sol = list(sol[0])
|
||||
else: print('IK could not find a solution!')
|
||||
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=[0.2 * i for i in self.joint_velocity_limits])
|
||||
print(len(traj.q))
|
||||
print(len(traj.t))
|
||||
print(traj.t)
|
||||
print(traj.arrive)
|
||||
msg.points = []
|
||||
for i in range(len(traj.q)):
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = list(traj.q[i])
|
||||
point.time_from_start.sec = int(traj.t[i])
|
||||
point.time_from_start.nanosec = int(((traj.t[i] - int(traj.t[i])) * 1e9))
|
||||
#point.time_from_start = rclpy.duration.Duration(seconds=traj.t[i]).to_msg()
|
||||
msg.points.append(point)
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
self.publisher.publish(msg)
|
||||
print('published')
|
||||
except Exception as e:
|
||||
print(f'Error in joint angles handler: {e}')
|
||||
|
||||
def main():
|
||||
"""Main function to get joint names and start the ROS 2 & OSC system."""
|
||||
while True:
|
||||
path_to_urdf = input("Enter the path to the URDF file: ")
|
||||
if os.path.isfile(path_to_urdf):
|
||||
if not path_to_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(path_to_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(path_to_urdf)
|
||||
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)
|
||||
|
||||
|
||||
|
||||
rclpy.init()
|
||||
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}")
|
||||
node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask, joint_velocity_limits)
|
||||
|
||||
# Run both ROS 2 and OSC Server together
|
||||
try:
|
||||
while rclpy.ok():
|
||||
osc_process() # Handle one OSC request at a time
|
||||
rclpy.spin_once(node, timeout_sec=0.1) # Process ROS callbacks
|
||||
except KeyboardInterrupt:
|
||||
print("")
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,180 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from sensor_msgs.msg import JointState
|
||||
from osc4py3.as_allthreads import *
|
||||
from osc4py3 import oscmethod as osm
|
||||
import xml.etree.ElementTree as ET
|
||||
import roboticstoolbox as rtb
|
||||
import spatialmath as sm
|
||||
import numpy as np
|
||||
import os
|
||||
|
||||
class ScaledJointTrajectoryPublisher(Node):
|
||||
"""Node to publish joint trajectories based on OSC messages."""
|
||||
def __init__(self, joint_names, robot, cost_mask, joint_velocity_limits):
|
||||
super().__init__('scaled_joint_trajectory_publisher')
|
||||
|
||||
self.joint_velocity_limits = joint_velocity_limits
|
||||
self.cost_mask = cost_mask
|
||||
self.robot = robot
|
||||
self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
|
||||
|
||||
if self.trajectroy_topic_name == "":
|
||||
self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
|
||||
print(f"Using topic name: {self.trajectroy_topic_name}")
|
||||
print("--------------------------------------------------------------------------------------------------------------------------------")
|
||||
while True:
|
||||
try:
|
||||
self.speed = input("Enter your desired speed of the tcp (in m/s): ")
|
||||
if self.speed == '':
|
||||
self.speed = 1
|
||||
else:
|
||||
self.speed = float(self.speed)
|
||||
break
|
||||
except ValueError:
|
||||
print("Invalid input. Please enter a number.")
|
||||
continue
|
||||
while True:
|
||||
try:
|
||||
self.t_acc = input("Enter how fast you want the tcp to reach that velocity (in s). \nRemember! If the acceleration time is to short the robot might not be able to accelerate fast enough: ")
|
||||
if self.t_acc == '':
|
||||
self.t_acc = 2
|
||||
else:
|
||||
self.t_acc = float(self.t_acc)
|
||||
break
|
||||
except ValueError:
|
||||
print("Invalid input. Please enter a number.")
|
||||
continue
|
||||
|
||||
|
||||
# ROS2 Publisher
|
||||
self.publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
self.trajectroy_topic_name,
|
||||
10
|
||||
)
|
||||
self.subscription = self.create_subscription(
|
||||
JointState,
|
||||
'/joint_states',
|
||||
self.joint_states_callback,
|
||||
1 # Increased queue size for joint states
|
||||
)
|
||||
# Store received joint positions
|
||||
self.joint_names = joint_names
|
||||
|
||||
self.port = 8000 # UDP port
|
||||
|
||||
osc_startup()
|
||||
osc_udp_server("0.0.0.0", self.port, "osc_server")
|
||||
print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
|
||||
# Register OSC handler
|
||||
osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
print("OSC method registered for /joint_trajectory")
|
||||
|
||||
def joint_states_callback(self, msg):
|
||||
"""Callback function to handle incoming joint states."""
|
||||
joint_position_dict = dict(zip(msg.name, msg.position))
|
||||
self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
|
||||
joint_velocity_dict = dict(zip(msg.name, msg.velocity))
|
||||
self.current_joint_velocities = [joint_velocity_dict[name] for name in self.joint_names]
|
||||
|
||||
|
||||
def joint_angles_handler(self, *args):
|
||||
"""Handles incoming OSC messages for joint positions."""
|
||||
try:
|
||||
print("Received joint positions")
|
||||
viapoints = np.array([list(i) for i in args])
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
x,y,z = self.robot.fkine(self.current_joint_positions).t
|
||||
r,p,yaw = self.robot.fkine(self.current_joint_positions).rpy()
|
||||
q0 = [x, y, z, r, p, yaw]
|
||||
traj = rtb.mstraj(viapoints, q0 = q0 ,dt=0.01, tacc=self.t_acc, qdmax=self.speed)
|
||||
msg.points = []
|
||||
prev_sol = self.current_joint_positions
|
||||
for i in range(len(traj.q)):
|
||||
T = sm.SE3(traj.q[i][:3]) * sm.SE3.RPY(traj.q[i][3:], order='xyz')
|
||||
sol = self.robot.ik_LM(T, q0=prev_sol, mask = self.cost_mask, joint_limits = True)
|
||||
if sol[1] == 1:
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = list(sol[0])
|
||||
point.time_from_start.sec = int(traj.t[i])
|
||||
point.time_from_start.nanosec = int((traj.t[i] - int(traj.t[i])) * 1e9)
|
||||
msg.points.append(point)
|
||||
prev_sol = list(sol[0])
|
||||
else: print('IK could not find a solution!')
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
self.publisher.publish(msg)
|
||||
print(f'lenght msg.points: {len(msg.points)}')
|
||||
|
||||
print('published')
|
||||
except Exception as e:
|
||||
print(f'Error in joint_angles_handler: {e}')
|
||||
|
||||
def main():
|
||||
"""Main function to get joint names and start the ROS 2 & OSC system."""
|
||||
while True:
|
||||
path_to_urdf = input("Enter the path to the URDF file: ")
|
||||
if os.path.isfile(path_to_urdf):
|
||||
if not path_to_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.")
|
||||
print("--------------------------------------------------------------------------------------------------------------------------------")
|
||||
tree = ET.parse(path_to_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(path_to_urdf)
|
||||
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)
|
||||
|
||||
|
||||
|
||||
rclpy.init()
|
||||
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("--------------------------------------------------------------------------------------------------------------------------------")
|
||||
node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask, joint_velocity_limits)
|
||||
|
||||
# Run both ROS 2 and OSC Server together
|
||||
try:
|
||||
while rclpy.ok():
|
||||
osc_process() # Handle one OSC request at a time
|
||||
rclpy.spin_once(node, timeout_sec=0.1) # Process ROS callbacks
|
||||
except KeyboardInterrupt:
|
||||
print("")
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,185 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from sensor_msgs.msg import JointState
|
||||
from osc4py3.as_allthreads import *
|
||||
from osc4py3 import oscmethod as osm
|
||||
import xml.etree.ElementTree as ET
|
||||
import roboticstoolbox as rtb
|
||||
import spatialmath as sm
|
||||
import numpy as np
|
||||
import time
|
||||
import os
|
||||
|
||||
class ScaledJointTrajectoryPublisher(Node):
|
||||
"""Node to publish joint trajectories based on OSC messages."""
|
||||
def __init__(self, joint_names, robot, cost_mask, joint_velocity_limits):
|
||||
super().__init__('scaled_joint_trajectory_publisher')
|
||||
|
||||
self.joint_velocity_limits = [joint_velocity_limits[joint] for joint in joint_names]
|
||||
self.cost_mask = cost_mask
|
||||
self.robot = robot
|
||||
self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
|
||||
|
||||
if self.trajectroy_topic_name == "":
|
||||
self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
|
||||
|
||||
# ROS2 Publisher
|
||||
self.publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
self.trajectroy_topic_name,
|
||||
10
|
||||
)
|
||||
self.subscription = self.create_subscription(
|
||||
JointState,
|
||||
'/joint_states',
|
||||
self.joint_states_callback,
|
||||
1 # Increased queue size for joint states
|
||||
)
|
||||
|
||||
|
||||
|
||||
|
||||
self.maximum_acceleration = [0.0] * len(joint_names)
|
||||
# Store received joint positions
|
||||
self.joint_names = joint_names
|
||||
for joint in joint_names:
|
||||
self.maximum_acceleration[joint_names.index(joint)] = float(input(f"Enter the maximum acceleration for joint {joint}: "))
|
||||
|
||||
self.port = 8000 # UDP port
|
||||
|
||||
osc_startup()
|
||||
osc_udp_server("0.0.0.0", self.port, "osc_server")
|
||||
print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
|
||||
# Register OSC handler
|
||||
osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
print("OSC method registered for /joint_trajectory")
|
||||
|
||||
def joint_states_callback(self, msg):
|
||||
"""Callback function to handle incoming joint states."""
|
||||
joint_position_dict = dict(zip(msg.name, msg.position))
|
||||
self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
|
||||
joint_velocity_dict = dict(zip(msg.name, msg.velocity))
|
||||
self.current_joint_velocities = [joint_velocity_dict[name] for name in self.joint_names]
|
||||
|
||||
def joint_angles_handler(self, *args):
|
||||
"""Handles incoming OSC messages for joint positions."""
|
||||
try:
|
||||
print("Received joint positions")
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = self.joint_names
|
||||
steps_per_m = 50
|
||||
|
||||
prev_duration = 0
|
||||
for i in range(len(args)-1):
|
||||
x, y, z, roll, pitch, yaw = args[i]
|
||||
x1, y1, z1, roll1, pitch1, yaw1 = args[i+1]
|
||||
steps = int(np.linalg.norm([x1-x, y1-y, z1-z])*steps_per_m)
|
||||
if steps <= 1: 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], order='xyz') for i in range(steps)]
|
||||
prev_sol = self.current_joint_positions if i == 0 else sol[0]
|
||||
sol_set = []
|
||||
for j in (range(steps) if i == 0 else range(1,steps)):
|
||||
sol = self.robot.ik_LM(cart_traj[j], q0=[0.0] * len(self.joint_names), mask = self.cost_mask, joint_limits = True) if i == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True)
|
||||
if sol[1] == 1:
|
||||
sol_set.append(sol[0])
|
||||
prev_sol = list(sol[0])
|
||||
else: print(f'IK could not find a solution for (x,y,z) = ({cart_traj[j].t}), (roll,pitch,yaw) = ({cart_traj[j].rpy()})!')
|
||||
distance = abs(sol_set[0]-sol_set[-1])
|
||||
ts= distance/np.array(self.joint_velocity_limits)+2*np.array(self.joint_velocity_limits)/np.array(self.maximum_acceleration)
|
||||
t = max(ts)
|
||||
idx = list(ts).index(t)
|
||||
s_acc = self.joint_velocity_limits[idx]**2/(2*self.maximum_acceleration[idx])
|
||||
print(f"t: {t}, idx: {idx}, s_acc: {s_acc}")
|
||||
print(f"sol_set: {sol_set}")
|
||||
|
||||
for sol in sol_set:
|
||||
print(f"sol: {sol}")
|
||||
s = abs(sol[idx]-sol_set[0][idx])
|
||||
print(f"sol_set[0][idx]: {sol_set[0][idx]}, sol[idx]: {sol[idx]}, s: {s}")
|
||||
if s <= s_acc:
|
||||
duration = np.sqrt(s/self.maximum_acceleration[idx])
|
||||
print(f"acceleration phase, duration: {duration}")
|
||||
elif s <= sol_set[-1][idx]-s_acc:
|
||||
duration = self.joint_velocity_limits[idx]/self.maximum_acceleration[idx] + (s-s_acc)/self.joint_velocity_limits[idx]
|
||||
print(f"constant velocity phase, duration: {duration}")
|
||||
else:
|
||||
duration = t-np.sqrt((sol_set[-1][idx]-s)/self.maximum_acceleration[idx])
|
||||
print(f"deceleration phase, duration: {duration}")
|
||||
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = list(sol)
|
||||
duration += prev_duration
|
||||
point.time_from_start.sec = int(duration)
|
||||
point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
|
||||
msg.points.append(point)
|
||||
prev_duration = duration
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
self.publisher.publish(msg)
|
||||
except Exception as e:
|
||||
print(f"Error in joint angles handler: {e}")
|
||||
|
||||
def main():
|
||||
"""Main function to get joint names and start the ROS 2 & OSC system."""
|
||||
while True:
|
||||
path_to_urdf = input("Enter the path to the URDF file: ")
|
||||
if os.path.isfile(path_to_urdf):
|
||||
if not path_to_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(path_to_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(path_to_urdf)
|
||||
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)
|
||||
|
||||
|
||||
|
||||
rclpy.init()
|
||||
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}")
|
||||
node = ScaledJointTrajectoryPublisher(joint_names, robot, cost_mask, joint_velocity_limits)
|
||||
|
||||
# Run both ROS 2 and OSC Server together
|
||||
try:
|
||||
while rclpy.ok():
|
||||
osc_process() # Handle one OSC request at a time
|
||||
rclpy.spin_once(node, timeout_sec=0.1) # Process ROS callbacks
|
||||
except KeyboardInterrupt:
|
||||
print("")
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,21 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>joint_control</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="root@todo.todo">root</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<depend>osc4py3</depend>
|
||||
<depend>trajectory_msgs</depend>
|
||||
<depend>xml</depend>
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/joint_control
|
||||
[install]
|
||||
install_scripts=$base/lib/joint_control
|
||||
@@ -1,42 +0,0 @@
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = 'joint_control'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
],
|
||||
install_requires=['setuptools'
|
||||
'osc4py3'],
|
||||
zip_safe=True,
|
||||
maintainer='root',
|
||||
maintainer_email='root@todo.todo',
|
||||
description='TODO: Package description',
|
||||
license='TODO: License declaration',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'joint_control = joint_control.joint_angles_server:main',
|
||||
'cart_coords = joint_control.cart_tcp_server:main',
|
||||
'trajectory_server = joint_control.trajectory_server:main',
|
||||
'trajectory_server_trapezoidal = joint_control.trajectory_server_trapezoidal:main',
|
||||
'trajectory_server_new = joint_control.trajectory_server_new:main',
|
||||
'trajectory_server_new_cart = joint_control.trajectory_server_new_cart:main',
|
||||
'trajectory_server_cart = joint_control.trajectory_server_cart:main',
|
||||
'trajectory_server_cart_fast = joint_control.trajectory_server_cart_fast:main',
|
||||
'trajectory_server_cart_fast_smooth = joint_control.trajectory_server_cart_fast_smooth:main',
|
||||
'trajectory_server_cart_fast_max_acc = joint_control.trajectory_server_cart_fast_max_acc:main',
|
||||
'plugdata = joint_control.plugdata:main',
|
||||
'plugdata_cart = joint_control.plugdata_cart:main',
|
||||
'plugdata_cart_fix = joint_control.plugdata_cart_fix:main',
|
||||
'plugdata_cart_smooth = joint_control.plugdata_cart_smooth:main',
|
||||
'test=joint_control.test:main',
|
||||
'sandbox=sandbox.sandbox:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -1,25 +0,0 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_copyright.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
# Remove the `skip` decorator once the source file(s) have a copyright header
|
||||
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
|
||||
@pytest.mark.copyright
|
||||
@pytest.mark.linter
|
||||
def test_copyright():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found errors'
|
||||
@@ -1,25 +0,0 @@
|
||||
# Copyright 2017 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_flake8.main import main_with_errors
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.flake8
|
||||
@pytest.mark.linter
|
||||
def test_flake8():
|
||||
rc, errors = main_with_errors(argv=[])
|
||||
assert rc == 0, \
|
||||
'Found %d code style errors / warnings:\n' % len(errors) + \
|
||||
'\n'.join(errors)
|
||||
@@ -1,23 +0,0 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_pep257.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.linter
|
||||
@pytest.mark.pep257
|
||||
def test_pep257():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found code style errors / warnings'
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,101 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
from osc4py3.as_eventloop import *
|
||||
from osc4py3 import oscbuildparse
|
||||
|
||||
class JointStateOSC(Node):
|
||||
def __init__(self):
|
||||
super().__init__('joint_states_osc')
|
||||
|
||||
# Create a ROS 2 subscriber to /joint_states topic
|
||||
self.subscription = self.create_subscription(
|
||||
JointState,
|
||||
'/joint_states',
|
||||
self.joint_states_callback,
|
||||
1 # Queue size
|
||||
)
|
||||
|
||||
# Open Sound Control (OSC) Client settings
|
||||
self.osc_ip = "127.0.0.1" # Replace with the target IP
|
||||
self.osc_port = 5005 # Replace with the target port
|
||||
# Start the OSC system
|
||||
osc_startup()
|
||||
|
||||
# Make client channels to send packets
|
||||
osc_udp_client(self.osc_ip, self.osc_port, "osc_client")
|
||||
|
||||
def joint_states_callback(self, msg):
|
||||
"""Callback function to handle incoming joint states."""
|
||||
header = msg.header
|
||||
joint_names = msg.name
|
||||
joint_positions = msg.position
|
||||
joint_velocity = msg.velocity
|
||||
joint_effort = msg.effort
|
||||
|
||||
joint_names_str = "\n- ".join(joint_names)
|
||||
joint_positions_str = "\n- ".join(map(str, joint_positions))
|
||||
joint_velocity_str = "\n- ".join(map(str, joint_velocity))
|
||||
joint_effort_str = "\n- ".join(map(str, joint_effort))
|
||||
|
||||
info = f"""
|
||||
---
|
||||
header:
|
||||
stamp:
|
||||
sec: {header.stamp.sec}
|
||||
nanosec: {header.stamp.nanosec}
|
||||
name:
|
||||
- {joint_names_str}
|
||||
position:
|
||||
- {joint_positions_str}
|
||||
velocity:
|
||||
- {joint_velocity_str}
|
||||
effort:
|
||||
- {joint_effort_str}
|
||||
---"""
|
||||
|
||||
# Send the info message
|
||||
msg_info = oscbuildparse.OSCMessage("/joint_states", None, [info])
|
||||
msg_name = oscbuildparse.OSCMessage("/joint_states/name", None, [i for i in joint_names])
|
||||
msg_position = oscbuildparse.OSCMessage("/joint_states/position", None, [i for i in joint_positions])
|
||||
msg_velocity = oscbuildparse.OSCMessage("/joint_states/velocity", None, [i for i in joint_velocity])
|
||||
msg_effort = oscbuildparse.OSCMessage("/joint_states/effort", None, [i for i in joint_effort])
|
||||
|
||||
bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_info, msg_name, msg_position, msg_velocity, msg_effort])
|
||||
|
||||
osc_send(bun, "osc_client")
|
||||
osc_process()
|
||||
#print(f"Publishing: {info}")
|
||||
|
||||
|
||||
'''
|
||||
# Send each joint state as an OSC message
|
||||
for i, name in enumerate(joint_names):
|
||||
#msg_sec = oscbuildparse.OSCMessage(f"/joint_states/header/sec", None, [header.stamp.sec])
|
||||
#msg_nanosec = oscbuildparse.OSCMessage(f"/joint_states/header/nanosec", None, [header.stamp.nanosec])
|
||||
msg_position = oscbuildparse.OSCMessage(f"/joint_states/{name}/position", None, [joint_positions[i]])
|
||||
msg_velocity = oscbuildparse.OSCMessage(f"/joint_states/{name}/velocity", None, [joint_velocity[i]])
|
||||
msg_effort = oscbuildparse.OSCMessage(f"/joint_states/{name}/effort", None, [joint_effort[i]])
|
||||
|
||||
bun = oscbuildparse.OSCBundle(oscbuildparse.unixtime2timetag(header.stamp.sec + header.stamp.nanosec), [msg_position, msg_velocity, msg_effort])
|
||||
#bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY , [msg_position, msg_velocity, msg_effort])
|
||||
osc_send(bun, "osc_client")
|
||||
osc_process()
|
||||
#print(f"OSC bundle sent for joint {name}")
|
||||
'''
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
node = JointStateOSC()
|
||||
print(f"Publishing joint states to OSC on {node.osc_ip}:{node.osc_port}...")
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
print("shutting down...")
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,41 +0,0 @@
|
||||
from osc4py3.as_eventloop import *
|
||||
from osc4py3 import oscmethod as osm
|
||||
import time
|
||||
|
||||
def joint_states_handler(address, *args):
|
||||
"""Handler function to process incoming joint states."""
|
||||
#print([i*180/3.141 for i in args]) # for printing joint angles in degrees
|
||||
|
||||
if address == "/joint_states":
|
||||
print(args[0])
|
||||
|
||||
def main():
|
||||
ip = "0.0.0.0" # IP address to listen on
|
||||
port = 8000 # Port to listen on
|
||||
|
||||
# Start the OSC system
|
||||
osc_startup()
|
||||
|
||||
# Make server channels to receive packets
|
||||
osc_udp_server(ip, port, "osc_server")
|
||||
|
||||
# Associate Python functions with message address patterns
|
||||
osc_method("/joint_states", joint_states_handler, argscheme=osm.OSCARG_ADDRESS + osm.OSCARG_DATAUNPACK)
|
||||
|
||||
print(f"Listening for OSC messages on {ip}:{port}...")
|
||||
|
||||
try:
|
||||
# Run the event loop
|
||||
while True:
|
||||
osc_process() # Process OSC messages
|
||||
time.sleep(0.01) # Sleep to avoid high CPU usage
|
||||
|
||||
except KeyboardInterrupt:
|
||||
print("")
|
||||
|
||||
finally:
|
||||
# Properly close the system
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,75 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
from osc4py3.as_eventloop import *
|
||||
from osc4py3 import oscbuildparse
|
||||
import roboticstoolbox as rtb
|
||||
import xml.etree.ElementTree as ET
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
|
||||
class JointStateOSC(Node):
|
||||
def __init__(self, robot, joint_names):
|
||||
super().__init__('joint_states_osc')
|
||||
|
||||
self.joint_names_urdf = joint_names
|
||||
self.robot = robot
|
||||
|
||||
# Create a ROS 2 subscriber to /joint_states topic
|
||||
self.subscription = self.create_subscription(
|
||||
JointState,
|
||||
'/joint_states',
|
||||
self.joint_states_callback,
|
||||
1 # Queue size
|
||||
)
|
||||
|
||||
# Open Sound Control (OSC) Client settings
|
||||
self.osc_ip = "127.0.0.1" # Replace with the target IP
|
||||
self.osc_port = 8000 # Replace with the target port
|
||||
|
||||
# Start the OSC system
|
||||
osc_startup()
|
||||
|
||||
# Make client channels to send packets
|
||||
osc_udp_client(self.osc_ip, self.osc_port, "osc_client")
|
||||
|
||||
def joint_states_callback(self, msg):
|
||||
"""Callback function to handle incoming joint states."""
|
||||
header = msg.header
|
||||
joint_names = msg.name
|
||||
joint_positions = msg.position
|
||||
joint_positions = [joint_positions[joint_names.index(joint)] for joint in self.joint_names_urdf]
|
||||
tcp_pos = self.robot.fkine(joint_positions) #, end='ft_frame')
|
||||
tcp_xyz = tcp_pos.t
|
||||
tcp_rot = tcp_pos.R
|
||||
rotation_vector = R.from_matrix(tcp_rot).as_rotvec()
|
||||
translation = oscbuildparse.OSCMessage("/tcp_position_t", None, tcp_xyz.tolist())
|
||||
osc_send(translation, "osc_client")
|
||||
rotation = oscbuildparse.OSCMessage("/tcp_position_R", None, rotation_vector.tolist())
|
||||
osc_send(rotation, "osc_client")
|
||||
osc_process()
|
||||
#print(f"Published TCP position: {tcp_pos}")
|
||||
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
robot = rtb.ERobot.URDF('/BA/robot.urdf')
|
||||
tree = ET.parse('/BA/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']
|
||||
|
||||
node = JointStateOSC(robot, joint_names)
|
||||
|
||||
print(f"Publishing TCP coordinates to OSC on {node.osc_ip}:{node.osc_port}...")
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
print("shutting down...")
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,24 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>joint_info</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="root@todo.todo">root</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>osc4py3</depend>
|
||||
<depend>roboticstoolbox</depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/joint_info
|
||||
[install]
|
||||
install_scripts=$base/lib/joint_info
|
||||
@@ -1,30 +0,0 @@
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = 'joint_info'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
],
|
||||
install_requires=[
|
||||
'setuptools'
|
||||
'osc4py3'],
|
||||
zip_safe=True,
|
||||
maintainer='root',
|
||||
maintainer_email='root@todo.todo',
|
||||
description='TODO: Package description',
|
||||
license='TODO: License declaration',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
"joint_states_pub = joint_info.osc_joint_states_pub:main",
|
||||
"joint_states_sub = joint_info.osc_joint_states_sub:main",
|
||||
"tcp_cart_pos = joint_info.tcp_cart_pos:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -1,25 +0,0 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_copyright.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
# Remove the `skip` decorator once the source file(s) have a copyright header
|
||||
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
|
||||
@pytest.mark.copyright
|
||||
@pytest.mark.linter
|
||||
def test_copyright():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found errors'
|
||||
@@ -1,25 +0,0 @@
|
||||
# Copyright 2017 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_flake8.main import main_with_errors
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.flake8
|
||||
@pytest.mark.linter
|
||||
def test_flake8():
|
||||
rc, errors = main_with_errors(argv=[])
|
||||
assert rc == 0, \
|
||||
'Found %d code style errors / warnings:\n' % len(errors) + \
|
||||
'\n'.join(errors)
|
||||
@@ -1,23 +0,0 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_pep257.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.linter
|
||||
@pytest.mark.pep257
|
||||
def test_pep257():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found code style errors / warnings'
|
||||
BIN
workspace/src/mock_robot/.DS_Store
vendored
BIN
workspace/src/mock_robot/.DS_Store
vendored
Binary file not shown.
BIN
workspace/src/mock_robot/mock_robot/.DS_Store
vendored
BIN
workspace/src/mock_robot/mock_robot/.DS_Store
vendored
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,38 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot name="mock_robot">
|
||||
|
||||
<!-- Base Link -->
|
||||
<link name="base_link"/>
|
||||
|
||||
<!-- Link 1 -->
|
||||
<link name="link1"/>
|
||||
|
||||
<joint name="joint1" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="link1"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="10.0" velocity="1.0" lower="-3.14" upper="3.14"/>
|
||||
</joint>
|
||||
|
||||
<!-- Link 2 -->
|
||||
<link name="link2"/>
|
||||
|
||||
<joint name="joint2" type="revolute">
|
||||
<parent link="link1"/>
|
||||
<child link="link2"/>
|
||||
<origin xyz="0.5 0 0" rpy="0 0 0"/> <!-- 0.5m offset -->
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="10.0" velocity="1.0" lower="-3.14" upper="3.14"/>
|
||||
</joint>
|
||||
|
||||
<!-- Tool endpoint -->
|
||||
<link name="tool0"/>
|
||||
|
||||
<joint name="tool0_fixed_joint" type="fixed">
|
||||
<parent link="link2"/>
|
||||
<child link="tool0"/>
|
||||
<origin xyz="0.3 0 0" rpy="0 0 0"/> <!-- TCP 0.3m from joint2 -->
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
@@ -1,73 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
from trajectory_msgs.msg import JointTrajectory
|
||||
|
||||
import numpy as np
|
||||
import time
|
||||
|
||||
class RobotNode(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('robot_node')
|
||||
|
||||
self.l1 = 0.5
|
||||
self.l2 = 0.3
|
||||
self.x = self.l1 + self.l2
|
||||
self.y = 0.0
|
||||
self.theta1 = 0.0
|
||||
self.theta2 = 0.0
|
||||
|
||||
self.publisher = self.create_publisher(
|
||||
JointState,
|
||||
'/joint_states',
|
||||
1
|
||||
)
|
||||
|
||||
timer_period = 0.01 # seconds
|
||||
self.timer = self.create_timer(timer_period, self.timer_callback)
|
||||
self.i = 0
|
||||
|
||||
self.subcriber = self.create_subscription(
|
||||
JointTrajectory,
|
||||
'/scaled_joint_trajectory_controller/joint_trajectory',
|
||||
self.joint_trajectory_callback,
|
||||
1
|
||||
)
|
||||
|
||||
def timer_callback(self):
|
||||
msg = JointState()
|
||||
msg.name = ['joint1', 'joint2']
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.position = [self.theta1, self.theta2]
|
||||
msg.velocity = [self.x, self.y]
|
||||
self.publisher.publish(msg)
|
||||
|
||||
def joint_trajectory_callback(self, msg):
|
||||
joint_names = msg.joint_names
|
||||
prev_timetag = 0
|
||||
for point in msg.points:
|
||||
duration = point.time_from_start.sec + point.time_from_start.nanosec / 1e9 - prev_timetag
|
||||
prev_timetag = point.time_from_start.sec + point.time_from_start.nanosec / 1e9
|
||||
|
||||
steps = int(duration * 10) # Update in 10 steps per second
|
||||
|
||||
for i in range(steps):
|
||||
self.theta1 += (point.positions[0] - self.theta1) / steps
|
||||
self.theta2 += (point.positions[1] - self.theta2) / steps
|
||||
self.x = self.l1 * np.cos(self.theta1) + self.l2 * np.cos(self.theta1 + self.theta2)
|
||||
self.y = self.l1 * np.sin(self.theta1) + self.l2 * np.sin(self.theta1 + self.theta2)
|
||||
msg = JointState()
|
||||
msg.name = ['joint1', 'joint2']
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.position = [self.theta1, self.theta2]
|
||||
msg.velocity = [self.x, self.y]
|
||||
self.publisher.publish(msg)
|
||||
time.sleep(0.1)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = RobotNode()
|
||||
rclpy.spin(node)
|
||||
rclpy.shutdown()
|
||||
@@ -1,20 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>mock_robot</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="root@todo.todo">root</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/mock_robot
|
||||
[install]
|
||||
install_scripts=$base/lib/mock_robot
|
||||
@@ -1,26 +0,0 @@
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = 'mock_robot'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='root',
|
||||
maintainer_email='root@todo.todo',
|
||||
description='TODO: Package description',
|
||||
license='TODO: License declaration',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'mock_robot = mock_robot.robot_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -1,25 +0,0 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_copyright.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
# Remove the `skip` decorator once the source file(s) have a copyright header
|
||||
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
|
||||
@pytest.mark.copyright
|
||||
@pytest.mark.linter
|
||||
def test_copyright():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found errors'
|
||||
@@ -1,25 +0,0 @@
|
||||
# Copyright 2017 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_flake8.main import main_with_errors
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.flake8
|
||||
@pytest.mark.linter
|
||||
def test_flake8():
|
||||
rc, errors = main_with_errors(argv=[])
|
||||
assert rc == 0, \
|
||||
'Found %d code style errors / warnings:\n' % len(errors) + \
|
||||
'\n'.join(errors)
|
||||
@@ -1,23 +0,0 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_pep257.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.linter
|
||||
@pytest.mark.pep257
|
||||
def test_pep257():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found code style errors / warnings'
|
||||
@@ -1,770 +0,0 @@
|
||||
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()
|
||||
@@ -1,811 +0,0 @@
|
||||
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()
|
||||
@@ -1,761 +0,0 @@
|
||||
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
|
||||
|
||||
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
|
||||
|
||||
|
||||
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)
|
||||
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
|
||||
|
||||
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
|
||||
print("tcp_coordinates_handler")
|
||||
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]
|
||||
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
|
||||
|
||||
def trapezoidal_timestamps(self, num_points, total_duration, flat_ratio):
|
||||
"""
|
||||
Generate symmetric timestamps with increasing → decreasing → flat → increasing spacing pattern.
|
||||
|
||||
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[float]: List of timestamps from 0 to total_duration.
|
||||
"""
|
||||
if num_points < 3:
|
||||
raise ValueError("Need at least 3 points for symmetry.")
|
||||
|
||||
# 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
|
||||
|
||||
# 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]])
|
||||
|
||||
# Normalize durations so they sum to total_duration
|
||||
durations *= total_duration / durations.sum()
|
||||
|
||||
# Convert to timestamps
|
||||
timestamps = np.cumsum(np.insert(durations, 0, 0.0))
|
||||
|
||||
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
|
||||
[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]
|
||||
#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], 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:
|
||||
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.prev_pose == 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:]
|
||||
'''
|
||||
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
|
||||
|
||||
def send_cartesian_trajectory(self):
|
||||
pass
|
||||
|
||||
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
|
||||
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:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
BIN
workspace/src/painting_robot_control/.DS_Store
vendored
BIN
workspace/src/painting_robot_control/.DS_Store
vendored
Binary file not shown.
@@ -1,20 +0,0 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>painting_robot_control</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="root@todo.todo">root</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,86 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Int32
|
||||
from math import sin, cos, pi
|
||||
from trajectory_msgs.msg import JointTrajectory
|
||||
import time
|
||||
|
||||
# Adjusted joint limits based on nearest multiple of step angle
|
||||
|
||||
|
||||
|
||||
class ArmControllerNode(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('arm_controller_node')
|
||||
self.subscription = self.create_subscription(
|
||||
JointTrajectory,
|
||||
'/scaled_joint_trajectory_controller/joint_trajectory',
|
||||
self.joint_trajectory_callback,
|
||||
100
|
||||
)
|
||||
self.dq1_publisher = self.create_publisher(Int32, 'dq1_steps', 10)
|
||||
self.dq2_publisher = self.create_publisher(Int32, 'dq2_steps', 10)
|
||||
|
||||
self.q1_min = -1.57
|
||||
self.q1_max = 1
|
||||
self.q2_min = 0.02
|
||||
self.q2_max = 2.8
|
||||
|
||||
self.current_q1 = 0.00
|
||||
self.current_q2 = 1.57 # Initial joint angles
|
||||
steps_per_revolution = 200
|
||||
gear_ratio = 19.2
|
||||
self.steps_per_radian = (steps_per_revolution * gear_ratio) / (2 * pi)
|
||||
|
||||
def joint_trajectory_callback(self, msg):
|
||||
prev_timetag = 0
|
||||
for point in msg.points:
|
||||
timetag = point.time_from_start.sec + point.time_from_start.nanosec / 1e9 - prev_timetag
|
||||
prev_timetag = point.time_from_start.sec + point.time_from_start.nanosec / 1e9
|
||||
|
||||
new_q1=max(min(point.positions[0],self.q1_max),self.q1_min)
|
||||
new_q2=max(min(point.positions[1],self.q2_max),self.q2_min)
|
||||
dq1 = new_q1 - self.current_q1
|
||||
dq2 = new_q2 - self.current_q2
|
||||
dq1_steps = int(round(dq1 * self.steps_per_radian))
|
||||
dq2_steps = int(round(dq2 * self.steps_per_radian))
|
||||
|
||||
self.current_q1 += dq1_steps/self.steps_per_radian
|
||||
self.current_q2 += dq2_steps/self.steps_per_radian
|
||||
|
||||
dq1_steps_msg = Int32()
|
||||
dq1_steps_msg.data = dq1_steps
|
||||
self.dq1_publisher.publish(dq1_steps_msg)
|
||||
|
||||
dq2_steps_msg = Int32()
|
||||
dq2_steps_msg.data = dq2_steps
|
||||
self.dq2_publisher.publish(dq2_steps_msg)
|
||||
x= 0.4 * cos(self.current_q1) + 0.25025 * cos(self.current_q1+self.current_q2)
|
||||
y= 0.4 * sin(self.current_q1) + 0.25025 * sin(self.current_q1+self.current_q2)
|
||||
|
||||
self.get_logger().info(f"Steps taken: steps_q1 = {dq1_steps}, steps_q2 = {dq2_steps}")
|
||||
self.get_logger().info(f"New joint positions (in radians): q1 = {self.current_q1}, q2 = {self.current_q2}")
|
||||
self.get_logger().info(f"New x_y positions (in meters): x = {x}, y = {y}")
|
||||
self.get_logger().info(f"Duration: {timetag}")
|
||||
time.sleep(timetag)
|
||||
|
||||
|
||||
|
||||
|
||||
def main(args=None):
|
||||
try:
|
||||
rclpy.init(args=args)
|
||||
node = ArmControllerNode()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
print('Communication to painting robot closed')
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
||||
|
||||
|
||||
@@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/painting_robot_control
|
||||
[install]
|
||||
install_scripts=$base/lib/painting_robot_control
|
||||
@@ -1,26 +0,0 @@
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = 'painting_robot_control'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='root',
|
||||
maintainer_email='root@todo.todo',
|
||||
description='TODO: Package description',
|
||||
license='TODO: License declaration',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'painting_robot_controller = painting_robot_control.com_node:main'
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -1,25 +0,0 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_copyright.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
# Remove the `skip` decorator once the source file(s) have a copyright header
|
||||
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
|
||||
@pytest.mark.copyright
|
||||
@pytest.mark.linter
|
||||
def test_copyright():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found errors'
|
||||
@@ -1,25 +0,0 @@
|
||||
# Copyright 2017 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_flake8.main import main_with_errors
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.flake8
|
||||
@pytest.mark.linter
|
||||
def test_flake8():
|
||||
rc, errors = main_with_errors(argv=[])
|
||||
assert rc == 0, \
|
||||
'Found %d code style errors / warnings:\n' % len(errors) + \
|
||||
'\n'.join(errors)
|
||||
@@ -1,23 +0,0 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_pep257.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.linter
|
||||
@pytest.mark.pep257
|
||||
def test_pep257():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found code style errors / warnings'
|
||||
Reference in New Issue
Block a user