remove gitignore
This commit is contained in:
@@ -0,0 +1,108 @@
|
||||
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()
|
||||
@@ -0,0 +1,80 @@
|
||||
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."""
|
||||
if len(args) == len(self.joint_positions):
|
||||
self.joint_positions = args
|
||||
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=3.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: {self.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()
|
||||
@@ -0,0 +1,78 @@
|
||||
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()
|
||||
@@ -0,0 +1,106 @@
|
||||
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):
|
||||
super().__init__('scaled_joint_trajectory_publisher')
|
||||
|
||||
self.robot = robot
|
||||
|
||||
# ROS2 Publisher
|
||||
self.publisher = self.create_publisher(
|
||||
JointTrajectory,
|
||||
'/scaled_joint_trajectory_controller/joint_trajectory',
|
||||
10
|
||||
)
|
||||
|
||||
# Store received joint positions
|
||||
self.joint_names = joint_names
|
||||
|
||||
osc_startup()
|
||||
osc_udp_server("0.0.0.0", 6080, "osc_server")
|
||||
print("Server started on 0.0.0.0:8000 \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 len(args[0]) == len(self.joint_names):
|
||||
n=2.0
|
||||
for i in range(len(args)-1):
|
||||
x, y, z, roll, pitch, yaw = args[i]
|
||||
Tep1 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
|
||||
x, y, z, roll, pitch, yaw = args[i+1]
|
||||
Tep2 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
|
||||
cart_traj = rtb.ctraj(Tep1, Tep2, steps)
|
||||
for j in range(steps-1):
|
||||
sol = self.robot.ik_LM(cart_traj[j], q0=joint_positions)
|
||||
dist = np.linalg.norm(cart_traj[j].t - cart_traj[j+1].t)
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = list(sol[0])
|
||||
joint_positions = list(sol[0])
|
||||
point.time_from_start.sec = int(n)
|
||||
point.time_from_start.nanosec = int((n - int(n)) * 1e9)
|
||||
n+=dist/vel
|
||||
n+=0.1
|
||||
msg.points.append(point)
|
||||
|
||||
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")
|
||||
|
||||
self.publisher.publish(msg)
|
||||
print("published joint positions")
|
||||
print(f'Frequency: {round(1/(time.time()-time1),2)} Hz')
|
||||
|
||||
|
||||
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(joint_names, robot)
|
||||
|
||||
# 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()
|
||||
@@ -0,0 +1,101 @@
|
||||
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 = 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_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()
|
||||
@@ -0,0 +1,41 @@
|
||||
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()
|
||||
@@ -0,0 +1,75 @@
|
||||
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()
|
||||
@@ -0,0 +1,63 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
|
||||
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',
|
||||
10
|
||||
)
|
||||
|
||||
timer_period = 0.5 # seconds
|
||||
self.timer = self.create_timer(timer_period, self.timer_callback)
|
||||
self.i = 0
|
||||
|
||||
def timer_callback(self):
|
||||
msg = JointState()
|
||||
msg.name = ['joint1', 'joint2']
|
||||
msg.position = [self.theta1, self.theta2]
|
||||
self.publisher.publish(msg)
|
||||
self.get_logger().info(f'x = {self.x}, y = {self.y}, theta1 = {self.theta1}, theta2 = {self.theta2}')
|
||||
|
||||
|
||||
self.subcriber = self.create_subscription(
|
||||
JointTrajectory,
|
||||
'/joint_trajectory_controller/joint_trajectory',
|
||||
self.joint_trajectory_callback,
|
||||
10
|
||||
)
|
||||
|
||||
def joint_trajectory_callback(self, msg):
|
||||
joint_names = msg.joint_names
|
||||
duration = 0
|
||||
for point in msg.points:
|
||||
duration = msg.time_from_start_sec + msg.time_from_start_nanosec / 1e9 - duration
|
||||
for i in range(duration *10):
|
||||
self.theta1 += (point.positions[0] - self.theta1) / 10
|
||||
self.theta2 += (point.positions[1] - self.theta2) / 10
|
||||
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)
|
||||
time.sleep(0.1)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = RobotNode()
|
||||
rclpy.spin(node)
|
||||
rclpy.shutdown()
|
||||
1
workspace/build/mock_robot/colcon_build.rc
Normal file
1
workspace/build/mock_robot/colcon_build.rc
Normal file
@@ -0,0 +1 @@
|
||||
0
|
||||
@@ -0,0 +1 @@
|
||||
# generated from colcon_core/shell/template/command_prefix.sh.em
|
||||
@@ -0,0 +1,20 @@
|
||||
AMENT_PREFIX_PATH=/BA/workspace/install/painting_robot_control:/BA/workspace/install/mock_robot:/BA/workspace/install/joint_info:/BA/workspace/install/joint_control:/opt/ros/humble
|
||||
COLCON=1
|
||||
COLCON_PREFIX_PATH=/BA/workspace/install
|
||||
HOME=/root
|
||||
HOSTNAME=0e38e264ac6b
|
||||
LANG=C.UTF-8
|
||||
LC_ALL=C.UTF-8
|
||||
LD_LIBRARY_PATH=/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib
|
||||
LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.webp=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36:
|
||||
OLDPWD=/BA/workspace/src
|
||||
PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin
|
||||
PWD=/BA/workspace/build/mock_robot
|
||||
PYTHONPATH=/BA/workspace/build/painting_robot_control:/BA/workspace/install/painting_robot_control/lib/python3.10/site-packages:/BA/workspace/build/mock_robot:/BA/workspace/install/mock_robot/lib/python3.10/site-packages:/BA/workspace/install/joint_info/lib/python3.10/site-packages:/BA/workspace/build/joint_control:/BA/workspace/install/joint_control/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages
|
||||
ROS_DISTRO=humble
|
||||
ROS_LOCALHOST_ONLY=0
|
||||
ROS_PYTHON_VERSION=3
|
||||
ROS_VERSION=2
|
||||
SHLVL=1
|
||||
TERM=xterm
|
||||
_=/usr/bin/colcon
|
||||
1
workspace/build/mock_robot/mock_robot
Symbolic link
1
workspace/build/mock_robot/mock_robot
Symbolic link
@@ -0,0 +1 @@
|
||||
/BA/workspace/src/mock_robot/mock_robot
|
||||
12
workspace/build/mock_robot/mock_robot.egg-info/PKG-INFO
Normal file
12
workspace/build/mock_robot/mock_robot.egg-info/PKG-INFO
Normal file
@@ -0,0 +1,12 @@
|
||||
Metadata-Version: 2.1
|
||||
Name: mock-robot
|
||||
Version: 0.0.0
|
||||
Summary: TODO: Package description
|
||||
Home-page: UNKNOWN
|
||||
Maintainer: root
|
||||
Maintainer-email: root@todo.todo
|
||||
License: TODO: License declaration
|
||||
Platform: UNKNOWN
|
||||
|
||||
UNKNOWN
|
||||
|
||||
20
workspace/build/mock_robot/mock_robot.egg-info/SOURCES.txt
Normal file
20
workspace/build/mock_robot/mock_robot.egg-info/SOURCES.txt
Normal file
@@ -0,0 +1,20 @@
|
||||
package.xml
|
||||
setup.cfg
|
||||
setup.py
|
||||
../../build/mock_robot/mock_robot.egg-info/PKG-INFO
|
||||
../../build/mock_robot/mock_robot.egg-info/SOURCES.txt
|
||||
../../build/mock_robot/mock_robot.egg-info/dependency_links.txt
|
||||
../../build/mock_robot/mock_robot.egg-info/entry_points.txt
|
||||
../../build/mock_robot/mock_robot.egg-info/requires.txt
|
||||
../../build/mock_robot/mock_robot.egg-info/top_level.txt
|
||||
../../build/mock_robot/mock_robot.egg-info/zip-safe
|
||||
mock_robot/__init__.py
|
||||
mock_robot/robot_node.py
|
||||
mock_robot.egg-info/PKG-INFO
|
||||
mock_robot.egg-info/SOURCES.txt
|
||||
mock_robot.egg-info/dependency_links.txt
|
||||
mock_robot.egg-info/entry_points.txt
|
||||
mock_robot.egg-info/requires.txt
|
||||
mock_robot.egg-info/top_level.txt
|
||||
mock_robot.egg-info/zip-safe
|
||||
resource/mock_robot
|
||||
@@ -0,0 +1 @@
|
||||
|
||||
@@ -0,0 +1,3 @@
|
||||
[console_scripts]
|
||||
mock_robot = mock_robot.robot_node:main
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
setuptools
|
||||
@@ -0,0 +1 @@
|
||||
mock_robot
|
||||
1
workspace/build/mock_robot/mock_robot.egg-info/zip-safe
Normal file
1
workspace/build/mock_robot/mock_robot.egg-info/zip-safe
Normal file
@@ -0,0 +1 @@
|
||||
|
||||
1
workspace/build/mock_robot/package.xml
Symbolic link
1
workspace/build/mock_robot/package.xml
Symbolic link
@@ -0,0 +1 @@
|
||||
/BA/workspace/src/mock_robot/package.xml
|
||||
Binary file not shown.
@@ -0,0 +1,4 @@
|
||||
import sys
|
||||
if sys.prefix == '/usr':
|
||||
sys.real_prefix = sys.prefix
|
||||
sys.prefix = sys.exec_prefix = '/BA/workspace/install/mock_robot'
|
||||
1
workspace/build/mock_robot/resource/mock_robot
Symbolic link
1
workspace/build/mock_robot/resource/mock_robot
Symbolic link
@@ -0,0 +1 @@
|
||||
/BA/workspace/src/mock_robot/resource/mock_robot
|
||||
1
workspace/build/mock_robot/setup.cfg
Symbolic link
1
workspace/build/mock_robot/setup.cfg
Symbolic link
@@ -0,0 +1 @@
|
||||
/BA/workspace/src/mock_robot/setup.cfg
|
||||
1
workspace/build/mock_robot/setup.py
Symbolic link
1
workspace/build/mock_robot/setup.py
Symbolic link
@@ -0,0 +1 @@
|
||||
/BA/workspace/src/mock_robot/setup.py
|
||||
@@ -0,0 +1 @@
|
||||
prepend-non-duplicate;PYTHONPATH;/BA/workspace/build/mock_robot
|
||||
@@ -0,0 +1,3 @@
|
||||
# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
|
||||
|
||||
colcon_prepend_unique_value PYTHONPATH "$env:COLCON_CURRENT_PREFIX\/BA/workspace/build/mock_robot"
|
||||
@@ -0,0 +1,3 @@
|
||||
# generated from colcon_core/shell/template/hook_prepend_value.sh.em
|
||||
|
||||
_colcon_prepend_unique_value PYTHONPATH "/BA/workspace/build/mock_robot"
|
||||
1
workspace/build/painting_robot_control/colcon_build.rc
Normal file
1
workspace/build/painting_robot_control/colcon_build.rc
Normal file
@@ -0,0 +1 @@
|
||||
0
|
||||
@@ -0,0 +1 @@
|
||||
# generated from colcon_core/shell/template/command_prefix.sh.em
|
||||
@@ -0,0 +1,20 @@
|
||||
AMENT_PREFIX_PATH=/BA/workspace/install/painting_robot_control:/BA/workspace/install/mock_robot:/BA/workspace/install/joint_info:/BA/workspace/install/joint_control:/opt/ros/humble
|
||||
COLCON=1
|
||||
COLCON_PREFIX_PATH=/BA/workspace/install
|
||||
HOME=/root
|
||||
HOSTNAME=0e38e264ac6b
|
||||
LANG=C.UTF-8
|
||||
LC_ALL=C.UTF-8
|
||||
LD_LIBRARY_PATH=/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib
|
||||
LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.webp=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36:
|
||||
OLDPWD=/BA/workspace/src
|
||||
PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin
|
||||
PWD=/BA/workspace/build/painting_robot_control
|
||||
PYTHONPATH=/BA/workspace/build/painting_robot_control:/BA/workspace/install/painting_robot_control/lib/python3.10/site-packages:/BA/workspace/build/mock_robot:/BA/workspace/install/mock_robot/lib/python3.10/site-packages:/BA/workspace/install/joint_info/lib/python3.10/site-packages:/BA/workspace/build/joint_control:/BA/workspace/install/joint_control/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages
|
||||
ROS_DISTRO=humble
|
||||
ROS_LOCALHOST_ONLY=0
|
||||
ROS_PYTHON_VERSION=3
|
||||
ROS_VERSION=2
|
||||
SHLVL=1
|
||||
TERM=xterm
|
||||
_=/usr/bin/colcon
|
||||
1
workspace/build/painting_robot_control/package.xml
Symbolic link
1
workspace/build/painting_robot_control/package.xml
Symbolic link
@@ -0,0 +1 @@
|
||||
/BA/workspace/src/painting_robot_control/package.xml
|
||||
1
workspace/build/painting_robot_control/painting_robot_control
Symbolic link
1
workspace/build/painting_robot_control/painting_robot_control
Symbolic link
@@ -0,0 +1 @@
|
||||
/BA/workspace/src/painting_robot_control/painting_robot_control
|
||||
@@ -0,0 +1,12 @@
|
||||
Metadata-Version: 2.1
|
||||
Name: painting-robot-control
|
||||
Version: 0.0.0
|
||||
Summary: TODO: Package description
|
||||
Home-page: UNKNOWN
|
||||
Maintainer: root
|
||||
Maintainer-email: root@todo.todo
|
||||
License: TODO: License declaration
|
||||
Platform: UNKNOWN
|
||||
|
||||
UNKNOWN
|
||||
|
||||
@@ -0,0 +1,13 @@
|
||||
package.xml
|
||||
setup.cfg
|
||||
setup.py
|
||||
painting_robot_control/__init__.py
|
||||
painting_robot_control/com_node.py
|
||||
painting_robot_control.egg-info/PKG-INFO
|
||||
painting_robot_control.egg-info/SOURCES.txt
|
||||
painting_robot_control.egg-info/dependency_links.txt
|
||||
painting_robot_control.egg-info/entry_points.txt
|
||||
painting_robot_control.egg-info/requires.txt
|
||||
painting_robot_control.egg-info/top_level.txt
|
||||
painting_robot_control.egg-info/zip-safe
|
||||
resource/painting_robot_control
|
||||
@@ -0,0 +1 @@
|
||||
|
||||
@@ -0,0 +1,3 @@
|
||||
[console_scripts]
|
||||
painting_robot_controller = painting_robot_control.com_node:main
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
setuptools
|
||||
@@ -0,0 +1 @@
|
||||
painting_robot_control
|
||||
@@ -0,0 +1 @@
|
||||
|
||||
Binary file not shown.
@@ -0,0 +1,4 @@
|
||||
import sys
|
||||
if sys.prefix == '/usr':
|
||||
sys.real_prefix = sys.prefix
|
||||
sys.prefix = sys.exec_prefix = '/BA/workspace/install/painting_robot_control'
|
||||
@@ -0,0 +1 @@
|
||||
/BA/workspace/src/painting_robot_control/resource/painting_robot_control
|
||||
1
workspace/build/painting_robot_control/setup.cfg
Symbolic link
1
workspace/build/painting_robot_control/setup.cfg
Symbolic link
@@ -0,0 +1 @@
|
||||
/BA/workspace/src/painting_robot_control/setup.cfg
|
||||
1
workspace/build/painting_robot_control/setup.py
Symbolic link
1
workspace/build/painting_robot_control/setup.py
Symbolic link
@@ -0,0 +1 @@
|
||||
/BA/workspace/src/painting_robot_control/setup.py
|
||||
@@ -0,0 +1 @@
|
||||
prepend-non-duplicate;PYTHONPATH;/BA/workspace/build/painting_robot_control
|
||||
@@ -0,0 +1,3 @@
|
||||
# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
|
||||
|
||||
colcon_prepend_unique_value PYTHONPATH "$env:COLCON_CURRENT_PREFIX\/BA/workspace/build/painting_robot_control"
|
||||
@@ -0,0 +1,3 @@
|
||||
# generated from colcon_core/shell/template/hook_prepend_value.sh.em
|
||||
|
||||
_colcon_prepend_unique_value PYTHONPATH "/BA/workspace/build/painting_robot_control"
|
||||
Reference in New Issue
Block a user