AS: code update
This commit is contained in:
parent
e8d48c0a4d
commit
b67b2a5174
@ -6,7 +6,7 @@ def handler(address,*args):
|
|||||||
|
|
||||||
osc_startup()
|
osc_startup()
|
||||||
osc_udp_server("0.0.0.0", 7000, "osc_server")
|
osc_udp_server("0.0.0.0", 7000, "osc_server")
|
||||||
osc_method("/joint_states/counter", handler, argscheme=osm.OSCARG_ADDRESS+ osm.OSCARG_DATAUNPACK)
|
osc_method("/time", handler, argscheme=osm.OSCARG_ADDRESS+ osm.OSCARG_DATAUNPACK)
|
||||||
|
|
||||||
|
|
||||||
while True:
|
while True:
|
||||||
|
@ -1,29 +0,0 @@
|
|||||||
import pybullet as p
|
|
||||||
|
|
||||||
# Start PyBullet simulation
|
|
||||||
physicsClient = p.connect(p.DIRECT) # or p.GUI for graphical interface
|
|
||||||
|
|
||||||
# Load your robot URDF
|
|
||||||
robot_id = p.loadURDF("/ur10e.urdf", useFixedBase=True)
|
|
||||||
|
|
||||||
# Set joint angles
|
|
||||||
joint_angles = [0, 0, 3.141/2, 0, 0, 0]
|
|
||||||
for i, angle in enumerate(joint_angles):
|
|
||||||
p.setJointMotorControl2(robot_id, i, p.POSITION_CONTROL, targetPosition=angle)
|
|
||||||
|
|
||||||
# Step the simulation to update the robot's state
|
|
||||||
p.stepSimulation()
|
|
||||||
|
|
||||||
# Check if any link is colliding
|
|
||||||
collision = False
|
|
||||||
for i in range(p.getNumBodies()):
|
|
||||||
contact_points = p.getContactPoints(robot_id)
|
|
||||||
print(contact_points)
|
|
||||||
if len(contact_points) > 0:
|
|
||||||
collision = True
|
|
||||||
break
|
|
||||||
|
|
||||||
if collision:
|
|
||||||
print("The robot has collided with itself.")
|
|
||||||
else:
|
|
||||||
print("No collision detected.")
|
|
26
test/commands_recording.py
Normal file
26
test/commands_recording.py
Normal file
@ -0,0 +1,26 @@
|
|||||||
|
from pythonosc.dispatcher import Dispatcher
|
||||||
|
from pythonosc import osc_server
|
||||||
|
import csv
|
||||||
|
import time
|
||||||
|
|
||||||
|
# CSV file setup
|
||||||
|
csv_file = open("osc_log.csv", mode="w", newline="")
|
||||||
|
csv_writer = csv.writer(csv_file)
|
||||||
|
csv_writer.writerow(["timestamp", "x", "y", "z", "roll", "pitch", "yaw"])
|
||||||
|
|
||||||
|
# Handler for OSC messages
|
||||||
|
def handle_pose(address, *args):
|
||||||
|
timestamp = time.time()
|
||||||
|
csv_writer.writerow([timestamp] + list(args))
|
||||||
|
csv_file.flush() # Optional: ensures data is written immediately
|
||||||
|
|
||||||
|
# Setup dispatcher and bind address
|
||||||
|
dispatcher = Dispatcher()
|
||||||
|
dispatcher.map("/tcp_coordinates", handle_pose) # Accept messages sent to address /pose
|
||||||
|
|
||||||
|
# Start server
|
||||||
|
ip = "0.0.0.0"
|
||||||
|
port = 8000
|
||||||
|
server = osc_server.ThreadingOSCUDPServer((ip, port), dispatcher)
|
||||||
|
print(f"Listening for OSC messages on {ip}:{port}...")
|
||||||
|
server.serve_forever()
|
14
test/get_ip.py
Normal file
14
test/get_ip.py
Normal file
@ -0,0 +1,14 @@
|
|||||||
|
import socket
|
||||||
|
|
||||||
|
def get_ip_address():
|
||||||
|
try:
|
||||||
|
# Connect to an external address — no data is actually sent
|
||||||
|
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||||
|
s.connect(("8.8.8.8", 80)) # Google's DNS
|
||||||
|
ip = s.getsockname()[0]
|
||||||
|
s.close()
|
||||||
|
return ip
|
||||||
|
except Exception:
|
||||||
|
return "127.0.0.1" # fallback to localhost
|
||||||
|
|
||||||
|
print("Local IP address:", get_ip_address())
|
@ -12,28 +12,31 @@ class ScaledJointTrajectoryPublisher(Node):
|
|||||||
10
|
10
|
||||||
)
|
)
|
||||||
|
|
||||||
def send_trajectory(self, joint_positions, duration=2.0):
|
try:
|
||||||
"""Publish a joint trajectory command to move the UR10e."""
|
msg = JointTrajectory()
|
||||||
msg = JointTrajectory()
|
msg.joint_names = [
|
||||||
msg.joint_names = [
|
"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
|
||||||
"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
|
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
|
||||||
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
|
]
|
||||||
]
|
|
||||||
|
|
||||||
point = JointTrajectoryPoint()
|
point = JointTrajectoryPoint()
|
||||||
point.positions = joint_positions # Target joint positions
|
point.positions = [2.0]*6 # Target joint positions
|
||||||
point.time_from_start.sec = int(duration) # Duration in seconds
|
point.velocities = [2.0] * 6
|
||||||
|
point.accelerations = [2.0] * 6
|
||||||
|
#point.time_from_start.sec = int(2)
|
||||||
|
#point.time_from_start.nanosec = int(0)
|
||||||
|
msg.points.append(point)
|
||||||
|
|
||||||
msg.points.append(point)
|
self.publisher.publish(msg)
|
||||||
|
except Exception as e:
|
||||||
self.publisher.publish(msg)
|
print(f"Error publishing joint trajectory: {e}")
|
||||||
|
|
||||||
def main():
|
def main():
|
||||||
rclpy.init()
|
rclpy.init()
|
||||||
node = ScaledJointTrajectoryPublisher()
|
node = ScaledJointTrajectoryPublisher()
|
||||||
|
|
||||||
# Move to a target position
|
# Move to a target position
|
||||||
node.send_trajectory([0.0]*6)
|
|
||||||
|
|
||||||
rclpy.spin(node)
|
rclpy.spin(node)
|
||||||
node.destroy_node()
|
node.destroy_node()
|
||||||
|
Binary file not shown.
@ -13,6 +13,7 @@ from osc4py3 import oscbuildparse
|
|||||||
import time
|
import time
|
||||||
import os
|
import os
|
||||||
import re
|
import re
|
||||||
|
import socket
|
||||||
|
|
||||||
class JointNameListener(Node):
|
class JointNameListener(Node):
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
@ -35,11 +36,25 @@ class OSC_ROS2_interface(Node):
|
|||||||
def __init__(self, joint_names, joint_velocity_limits, robot, cost_mask):
|
def __init__(self, joint_names, joint_velocity_limits, robot, cost_mask):
|
||||||
super().__init__('scaled_joint_trajectory_publisher')
|
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
|
# ROS2 Publisher
|
||||||
self.publisher = self.create_publisher(
|
self.publisher = self.create_publisher(
|
||||||
JointTrajectory,
|
JointTrajectory,
|
||||||
'/scaled_joint_trajectory_controller/joint_trajectory',
|
self.trajectory_topic_name,
|
||||||
1
|
1
|
||||||
)
|
)
|
||||||
|
|
||||||
@ -73,6 +88,7 @@ class OSC_ROS2_interface(Node):
|
|||||||
50: "FATAL",
|
50: "FATAL",
|
||||||
}
|
}
|
||||||
self.speed_scaling = 0.2
|
self.speed_scaling = 0.2
|
||||||
|
self.new = False
|
||||||
|
|
||||||
|
|
||||||
while True:
|
while True:
|
||||||
@ -182,9 +198,29 @@ class OSC_ROS2_interface(Node):
|
|||||||
continue
|
continue
|
||||||
|
|
||||||
if lower_limit:
|
if lower_limit:
|
||||||
lim[0][i] = float(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[1][i] = float(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
|
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(f"New limits for joint '{self.joint_names[i]}': [{self.robot.qlim[0][i]} {self.robot.qlim[1][i]}] rad")
|
||||||
print("-" * 50)
|
print("-" * 50)
|
||||||
@ -245,10 +281,11 @@ class OSC_ROS2_interface(Node):
|
|||||||
|
|
||||||
# Register OSC handler
|
# Register OSC handler
|
||||||
osc_method("/joint_positions", self.joint_positions_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
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("/joint_position/*", self.joint_position_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||||
osc_method("/tcp_coordinates", self.tcp_coordinates_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("/joint_trajectory", self.joint_trajectory_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||||
osc_method("/cartesian_trajectory", self.cartesian_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)
|
print('--' * 50)
|
||||||
self.hz = float(input("Enter the desired refresh frequency (Hz): "))
|
self.hz = float(input("Enter the desired refresh frequency (Hz): "))
|
||||||
print()
|
print()
|
||||||
@ -262,7 +299,7 @@ class OSC_ROS2_interface(Node):
|
|||||||
print()
|
print()
|
||||||
print('=-=' * 50)
|
print('=-=' * 50)
|
||||||
print()
|
print()
|
||||||
print(f'Ready to receive OSC messages on 0.0.0.0:{commands_port}')
|
print(f'Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{commands_port}')
|
||||||
print()
|
print()
|
||||||
print('=-=' * 50)
|
print('=-=' * 50)
|
||||||
print()
|
print()
|
||||||
@ -272,9 +309,26 @@ class OSC_ROS2_interface(Node):
|
|||||||
self.get_logger().info(f'Sending log messages to {log_ip}:{log_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(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):
|
def joint_trajectory_handler(self, *args):
|
||||||
pass
|
pass
|
||||||
|
|
||||||
|
def joint_position_handler(self, *args):
|
||||||
|
pass
|
||||||
|
|
||||||
def cartesian_trajectory_handler(self, *args):
|
def cartesian_trajectory_handler(self, *args):
|
||||||
"""Handles incoming OSC messages for cartesian trajectory."""
|
"""Handles incoming OSC messages for cartesian trajectory."""
|
||||||
if self.robot:
|
if self.robot:
|
||||||
@ -285,30 +339,34 @@ class OSC_ROS2_interface(Node):
|
|||||||
|
|
||||||
def joint_positions_handler(self, *args):
|
def joint_positions_handler(self, *args):
|
||||||
"""Handles incoming OSC messages for joint positions."""
|
"""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
|
||||||
|
|
||||||
if len(args) == len(self.joint_names):
|
# Check if joint positions exceed limits
|
||||||
desired_joint_positions = [float(i) for i in list(args)] + [None]
|
for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]): # Exclude duration if present
|
||||||
elif len(args) == len(self.joint_names) + 1:
|
if position < self.robot.qlim[0][i]:
|
||||||
desired_joint_positions = [float(i) for i in list(args)]
|
desired_joint_positions[i] = self.robot.qlim[0][i]
|
||||||
else:
|
self.get_logger().warn(
|
||||||
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)}.")
|
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
|
return
|
||||||
|
|
||||||
# Check if joint positions exceed limits
|
|
||||||
for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]): # Exclude duration if present
|
|
||||||
if position < self.robot.qlim[0][i]:
|
|
||||||
desired_joint_positions[i] = self.robot.qlim[0][i]
|
|
||||||
self.get_logger().warn(
|
|
||||||
f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[0][i]}."
|
|
||||||
)
|
|
||||||
elif position > self.robot.qlim[1][i]:
|
|
||||||
desired_joint_positions[i] = self.robot.qlim[1][i]
|
|
||||||
self.get_logger().warn(
|
|
||||||
f"joint_positions_handler:Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[1][i]}."
|
|
||||||
)
|
|
||||||
|
|
||||||
self.desired = ["joint_positions"] + desired_joint_positions
|
|
||||||
|
|
||||||
def tcp_coordinates_handler(self, *args):
|
def tcp_coordinates_handler(self, *args):
|
||||||
# Ensure the desired joint positions are within the specified limits
|
# Ensure the desired joint positions are within the specified limits
|
||||||
if self.robot:
|
if self.robot:
|
||||||
@ -342,6 +400,7 @@ class OSC_ROS2_interface(Node):
|
|||||||
)
|
)
|
||||||
|
|
||||||
self.desired = ["tcp_coordinates", x, y, z, r, p, yaw, duration]
|
self.desired = ["tcp_coordinates", x, y, z, r, p, yaw, duration]
|
||||||
|
self.new = True
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
self.get_logger().fatal(f"tcp_coordinates_handler: {e}")
|
self.get_logger().fatal(f"tcp_coordinates_handler: {e}")
|
||||||
else:
|
else:
|
||||||
@ -352,6 +411,8 @@ class OSC_ROS2_interface(Node):
|
|||||||
def joint_states_callback(self, msg: JointState):
|
def joint_states_callback(self, msg: JointState):
|
||||||
"""Callback function to handle incoming joint states."""
|
"""Callback function to handle incoming joint states."""
|
||||||
try:
|
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
|
if not(self.joint_names): self.joint_names = msg.name
|
||||||
joint_position_dict = dict(zip(msg.name, msg.position))
|
joint_position_dict = dict(zip(msg.name, msg.position))
|
||||||
self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
|
self.current_joint_positions = [joint_position_dict[name] for name in self.joint_names]
|
||||||
@ -554,23 +615,23 @@ class OSC_ROS2_interface(Node):
|
|||||||
def update_position(self):
|
def update_position(self):
|
||||||
"""Calls the appropriate function to update the robot's position."""
|
"""Calls the appropriate function to update the robot's position."""
|
||||||
try:
|
try:
|
||||||
if self.desired is None:
|
if self.desired is None or not(self.new):
|
||||||
return
|
|
||||||
if self.desired == self.previous_desired:
|
|
||||||
msg_log = oscbuildparse.OSCMessage(f"/log/INFO", ',isss', [20, str(time.time()) , 'interface - update_position', "Desired joint positions are the same as previous."])
|
|
||||||
osc_send(msg_log, "osc_client")
|
|
||||||
return
|
return
|
||||||
|
|
||||||
if self.desired[0] == "joint_positions":
|
if self.desired[0] == "joint_positions":
|
||||||
|
self.new = False
|
||||||
self.send_joint_positions()
|
self.send_joint_positions()
|
||||||
return
|
return
|
||||||
elif self.desired[0] == "tcp_coordinates":
|
elif self.desired[0] == "tcp_coordinates":
|
||||||
|
self.new = False
|
||||||
self.send_tcp_coordinates()
|
self.send_tcp_coordinates()
|
||||||
return
|
return
|
||||||
elif self.desired[0] == "joint_trajectory":
|
elif self.desired[0] == "joint_trajectory":
|
||||||
|
self.new = False
|
||||||
self.send_joint_trajectory()
|
self.send_joint_trajectory()
|
||||||
return
|
return
|
||||||
elif self.desired[0] == "cartesian_trajectory":
|
elif self.desired[0] == "cartesian_trajectory":
|
||||||
|
self.new = False
|
||||||
self.send_cartesian_trajectory()
|
self.send_cartesian_trajectory()
|
||||||
return
|
return
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
@ -622,7 +683,7 @@ def main():
|
|||||||
print("Invalid path. Please enter a valid path to the URDF file.")
|
print("Invalid path. Please enter a valid path to the URDF file.")
|
||||||
tree = ET.parse(robot_urdf)
|
tree = ET.parse(robot_urdf)
|
||||||
root = tree.getroot()
|
root = tree.getroot()
|
||||||
joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
|
joint_names = [link.name for link in robot.links if link.isjoint]
|
||||||
robot = rtb.ERobot.URDF(robot_urdf)
|
robot = rtb.ERobot.URDF(robot_urdf)
|
||||||
print(robot)
|
print(robot)
|
||||||
joint_velocity_limits = {}
|
joint_velocity_limits = {}
|
||||||
|
Loading…
Reference in New Issue
Block a user