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

AS: upload log recordings

Browse Source
This commit is contained in:
cha2080
2025-04-30 13:28:00 +02:00
parent ff3b2a03bf
commit cd1ae17d41
124 changed files with 150040 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files

209
workspace/build/joint_control/build/lib/joint_control/plugdata.py Normal file
View File

@@ -0,0 +1,209 @@
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()

158
workspace/build/joint_control/build/lib/joint_control/plugdata_cart.py Normal file
View File

@@ -0,0 +1,158 @@
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()

264
workspace/build/joint_control/build/lib/joint_control/plugdata_cart_fix.py Normal file
View File

@@ -0,0 +1,264 @@
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.")
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 *= 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)
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()

232
workspace/build/joint_control/build/lib/joint_control/plugdata_cart_smooth.py Normal file
View File

@@ -0,0 +1,232 @@
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()
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)
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()

36
workspace/build/joint_control/build/lib/joint_control/test.py Normal file
View File

@@ -0,0 +1,36 @@
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)

141
workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast.py Normal file
View File

@@ -0,0 +1,141 @@
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()

166
workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast_max_acc.py Normal file
View File

@@ -0,0 +1,166 @@
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()

152
workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast_smooth.py Normal file
View File

@@ -0,0 +1,152 @@
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()

166
workspace/build/joint_control/build/lib/joint_control/trajectory_server_new.py Normal file
View File

@@ -0,0 +1,166 @@
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=[1 * 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()

180
workspace/build/joint_control/build/lib/joint_control/trajectory_server_new_cart.py Normal file
View File

@@ -0,0 +1,180 @@
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()

185
workspace/build/joint_control/build/lib/joint_control/trajectory_server_trapezoidal.py Normal file
View File

@@ -0,0 +1,185 @@
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()

2
workspace/build/joint_control/colcon_build.rc
View File

@@ -1 +1 @@
0
1

11
workspace/build/joint_control/colcon_command_prefix_setup_py.sh.env
View File

@@ -1,16 +1,15 @@
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
AMENT_PREFIX_PATH=/opt/ros/humble
COLCON=1
COLCON_PREFIX_PATH=/BA/workspace/install
HOME=/root
HOSTNAME=0e38e264ac6b
HOSTNAME=hapticslab2
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/joint_control/joint_control
OLDPWD=/ws/src/ba-alexanderschaefer
PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin
PWD=/BA/workspace/build/joint_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/build/joint_info:/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
PWD=/ws/src/ba-alexanderschaefer/workspace/build/joint_control
PYTHONPATH=/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

2
workspace/build/joint_control/joint_control
View File

@@ -1 +1 @@
/BA/workspace/src/joint_control/joint_control
/ws/src/ba-alexanderschaefer/workspace/src/joint_control/joint_control

12
workspace/build/joint_control/joint_control.egg-info/SOURCES.txt
View File

@@ -24,11 +24,7 @@ joint_control/trajectory_server_cart_fast_smooth.py
joint_control/trajectory_server_new.py
joint_control/trajectory_server_new_cart.py
joint_control/trajectory_server_trapezoidal.py
joint_control.egg-info/PKG-INFO
joint_control.egg-info/SOURCES.txt
joint_control.egg-info/dependency_links.txt
joint_control.egg-info/entry_points.txt
joint_control.egg-info/requires.txt
joint_control.egg-info/top_level.txt
joint_control.egg-info/zip-safe
resource/joint_control
resource/joint_control
test/test_copyright.py
test/test_flake8.py
test/test_pep257.py

2
workspace/build/joint_control/package.xml
View File

@@ -1 +1 @@
/BA/workspace/src/joint_control/package.xml
/ws/src/ba-alexanderschaefer/workspace/src/joint_control/package.xml

BIN
workspace/build/joint_control/prefix_override/__pycache__/sitecustomize.cpython-310.pyc
View File

Binary file not shown.

2
workspace/build/joint_control/prefix_override/sitecustomize.py
View File

@@ -1,4 +1,4 @@
import sys
if sys.prefix == '/usr':
sys.real_prefix = sys.prefix
sys.prefix = sys.exec_prefix = '/BA/workspace/install/joint_control'
sys.prefix = sys.exec_prefix = '/ws/src/ba-alexanderschaefer/workspace/install/joint_control'

2
workspace/build/joint_control/resource/joint_control
View File

@@ -1 +1 @@
/BA/workspace/src/joint_control/resource/joint_control
/ws/src/ba-alexanderschaefer/workspace/src/joint_control/resource/joint_control

2
workspace/build/joint_control/setup.cfg
View File

@@ -1 +1 @@
/BA/workspace/src/joint_control/setup.cfg
/ws/src/ba-alexanderschaefer/workspace/src/joint_control/setup.cfg

1
workspace/build/joint_control/setup.py
View File

@@ -1 +0,0 @@
/BA/workspace/src/joint_control/setup.py

2
workspace/install/joint_control/lib/python3.10/site-packages/joint-control.egg-link
View File

@@ -1,2 +0,0 @@
/BA/workspace/build/joint_control
.

0
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__init__.py Normal file
View File

BIN
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__pycache__/__init__.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__pycache__/cart_tcp_server.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__pycache__/joint_angles_server.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__pycache__/plugdata.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__pycache__/plugdata_cart.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__pycache__/plugdata_cart_fix.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__pycache__/plugdata_cart_smooth.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__pycache__/test.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__pycache__/trajectory_server.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__pycache__/trajectory_server_cart.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__pycache__/trajectory_server_cart_fast.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__pycache__/trajectory_server_cart_fast_max_acc.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__pycache__/trajectory_server_cart_fast_smooth.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__pycache__/trajectory_server_new.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__pycache__/trajectory_server_new_cart.cpython-310.pyc Normal file
View File

Binary file not shown.

BIN
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__pycache__/trajectory_server_trapezoidal.cpython-310.pyc Normal file
View File

Binary file not shown.

108
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/cart_tcp_server.py Normal file
View File

@@ -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()

80
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/joint_angles_server.py Normal file
View File

@@ -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()

209
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata.py Normal file
View File

@@ -0,0 +1,209 @@
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()

158
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart.py Normal file
View File

@@ -0,0 +1,158 @@
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()

264
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart_fix.py Normal file
View File

@@ -0,0 +1,264 @@
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.")
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 *= 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)
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()

232
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart_smooth.py Normal file
View File

@@ -0,0 +1,232 @@
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()
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)
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()

36
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/test.py Normal file
View File

@@ -0,0 +1,36 @@
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)

78
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server.py Normal file
View File

@@ -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()

106
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart.py Normal file
View File

@@ -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()

141
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast.py Normal file
View File

@@ -0,0 +1,141 @@
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()

166
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast_max_acc.py Normal file
View File

@@ -0,0 +1,166 @@
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()

152
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast_smooth.py Normal file
View File

@@ -0,0 +1,152 @@
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()

166
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_new.py Normal file
View File

@@ -0,0 +1,166 @@
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=[1 * 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()

180
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_new_cart.py Normal file
View File

@@ -0,0 +1,180 @@
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()

185
workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_trapezoidal.py Normal file
View File

@@ -0,0 +1,185 @@
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()

2
workspace/install/local_setup.sh
View File

@@ -6,7 +6,7 @@
# since a plain shell script can't determine its own path when being sourced
# either use the provided COLCON_CURRENT_PREFIX
# or fall back to the build time prefix (if it exists)
_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/BA/workspace/install"
_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/ws/src/ba-alexanderschaefer/workspace/install"
if [ -z "$COLCON_CURRENT_PREFIX" ]; then
if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then
echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2

2
workspace/install/setup.sh
View File

@@ -7,7 +7,7 @@
# since a plain shell script can't determine its own path when being sourced
# either use the provided COLCON_CURRENT_PREFIX
# or fall back to the build time prefix (if it exists)
_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/BA/workspace/install
_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/ws/src/ba-alexanderschaefer/workspace/install
if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then
_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then

46
workspace/log/build_2025-04-30_09-37-17/events.log Normal file
View File

@@ -0,0 +1,46 @@
[0.000000] (-) TimerEvent: {}
[0.000262] (-) JobUnselected: {'identifier': 'joint_info'}
[0.000366] (-) JobUnselected: {'identifier': 'mock_robot'}
[0.000442] (-) JobUnselected: {'identifier': 'painting_robot_control'}
[0.000469] (joint_control) JobQueued: {'identifier': 'joint_control', 'dependencies': OrderedDict()}
[0.000483] (joint_control) JobStarted: {'identifier': 'joint_control'}
[0.099902] (-) TimerEvent: {}
[0.200080] (-) TimerEvent: {}
[0.300267] (-) TimerEvent: {}
[0.334110] (joint_control) Command: {'cmd': ['/usr/bin/python3', '-W', 'ignore:setup.py install is deprecated', '-W', 'ignore:easy_install command is deprecated', 'setup.py', 'develop', '--editable', '--build-directory', '/ws/src/ba-alexanderschaefer/workspace/build/joint_control/build', '--no-deps', 'symlink_data'], 'cwd': '/ws/src/ba-alexanderschaefer/workspace/build/joint_control', 'env': {'HOSTNAME': 'hapticslab2', 'SHLVL': '1', 'LD_LIBRARY_PATH': '/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib', 'HOME': '/root', 'OLDPWD': '/ws/src/ba-alexanderschaefer', 'ROS_PYTHON_VERSION': '3', 'ROS_DISTRO': 'humble', '_': '/usr/bin/colcon', 'ROS_VERSION': '2', 'TERM': 'xterm', 'ROS_LOCALHOST_ONLY': '0', 'PATH': '/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin', 'LANG': 'C.UTF-8', '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:', 'AMENT_PREFIX_PATH': '/opt/ros/humble', 'PWD': '/ws/src/ba-alexanderschaefer/workspace/build/joint_control', 'LC_ALL': 'C.UTF-8', 'PYTHONPATH': '/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/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', 'COLCON': '1'}, 'shell': False}
[0.400349] (-) TimerEvent: {}
[0.472261] (joint_control) StdoutLine: {'line': b'running develop\n'}
[0.500416] (-) TimerEvent: {}
[0.550100] (joint_control) StdoutLine: {'line': b'running egg_info\n'}
[0.550228] (joint_control) StdoutLine: {'line': b'writing joint_control.egg-info/PKG-INFO\n'}
[0.550297] (joint_control) StdoutLine: {'line': b'writing dependency_links to joint_control.egg-info/dependency_links.txt\n'}
[0.550427] (joint_control) StdoutLine: {'line': b'writing entry points to joint_control.egg-info/entry_points.txt\n'}
[0.550501] (joint_control) StdoutLine: {'line': b'writing requirements to joint_control.egg-info/requires.txt\n'}
[0.550553] (joint_control) StdoutLine: {'line': b'writing top-level names to joint_control.egg-info/top_level.txt\n'}
[0.551471] (joint_control) StdoutLine: {'line': b"reading manifest file 'joint_control.egg-info/SOURCES.txt'\n"}
[0.552149] (joint_control) StdoutLine: {'line': b"writing manifest file 'joint_control.egg-info/SOURCES.txt'\n"}
[0.552826] (joint_control) StdoutLine: {'line': b'running build_ext\n'}
[0.552915] (joint_control) StdoutLine: {'line': b'Creating /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint-control.egg-link (link to .)\n'}
[0.553306] (joint_control) StdoutLine: {'line': b'Installing cart_coords script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control\n'}
[0.553428] (joint_control) StdoutLine: {'line': b'Installing joint_control script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control\n'}
[0.553527] (joint_control) StdoutLine: {'line': b'Installing plugdata script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control\n'}
[0.553608] (joint_control) StdoutLine: {'line': b'Installing plugdata_cart script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control\n'}
[0.553702] (joint_control) StdoutLine: {'line': b'Installing plugdata_cart_fix script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control\n'}
[0.553782] (joint_control) StdoutLine: {'line': b'Installing plugdata_cart_smooth script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control\n'}
[0.553858] (joint_control) StdoutLine: {'line': b'Installing test script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control\n'}
[0.553931] (joint_control) StdoutLine: {'line': b'Installing trajectory_server script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control\n'}
[0.554007] (joint_control) StdoutLine: {'line': b'Installing trajectory_server_cart script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control\n'}
[0.554103] (joint_control) StdoutLine: {'line': b'Installing trajectory_server_cart_fast script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control\n'}
[0.554180] (joint_control) StdoutLine: {'line': b'Installing trajectory_server_cart_fast_max_acc script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control\n'}
[0.554258] (joint_control) StdoutLine: {'line': b'Installing trajectory_server_cart_fast_smooth script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control\n'}
[0.554333] (joint_control) StdoutLine: {'line': b'Installing trajectory_server_new script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control\n'}
[0.554405] (joint_control) StdoutLine: {'line': b'Installing trajectory_server_new_cart script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control\n'}
[0.554489] (joint_control) StdoutLine: {'line': b'Installing trajectory_server_trapezoidal script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control\n'}
[0.554611] (joint_control) StdoutLine: {'line': b'\n'}
[0.554653] (joint_control) StdoutLine: {'line': b'Installed /ws/src/ba-alexanderschaefer/workspace/build/joint_control\n'}
[0.554688] (joint_control) StdoutLine: {'line': b'running symlink_data\n'}
[0.554746] (joint_control) StdoutLine: {'line': b'symbolically linking /ws/src/ba-alexanderschaefer/workspace/build/joint_control/resource/joint_control -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/ament_index/resource_index/packages\n'}
[0.554783] (joint_control) StderrLine: {'line': b"error: [Errno 17] File exists: '/ws/src/ba-alexanderschaefer/workspace/build/joint_control/resource/joint_control' -> '/ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/ament_index/resource_index/packages/joint_control'\n"}
[0.567286] (joint_control) CommandEnded: {'returncode': 1}
[0.567485] (joint_control) JobEnded: {'identifier': 'joint_control', 'rc': 1}
[0.577562] (-) EventReactorShutdown: {}

2
workspace/log/build_2025-04-30_09-37-17/joint_control/command.log Normal file
View File

@@ -0,0 +1,2 @@
Invoking command in '/ws/src/ba-alexanderschaefer/workspace/build/joint_control': PYTHONPATH=/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py develop --editable --build-directory /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build --no-deps symlink_data
Invoked command in '/ws/src/ba-alexanderschaefer/workspace/build/joint_control' returned '1': PYTHONPATH=/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py develop --editable --build-directory /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build --no-deps symlink_data

1
workspace/log/build_2025-04-30_09-37-17/joint_control/stderr.log Normal file
View File

@@ -0,0 +1 @@
error: [Errno 17] File exists: '/ws/src/ba-alexanderschaefer/workspace/build/joint_control/resource/joint_control' -> '/ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/ament_index/resource_index/packages/joint_control'

30
workspace/log/build_2025-04-30_09-37-17/joint_control/stdout.log Normal file
View File

@@ -0,0 +1,30 @@
running develop
running egg_info
writing joint_control.egg-info/PKG-INFO
writing dependency_links to joint_control.egg-info/dependency_links.txt
writing entry points to joint_control.egg-info/entry_points.txt
writing requirements to joint_control.egg-info/requires.txt
writing top-level names to joint_control.egg-info/top_level.txt
reading manifest file 'joint_control.egg-info/SOURCES.txt'
writing manifest file 'joint_control.egg-info/SOURCES.txt'
running build_ext
Creating /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint-control.egg-link (link to .)
Installing cart_coords script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing joint_control script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing plugdata script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing plugdata_cart script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing plugdata_cart_fix script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing plugdata_cart_smooth script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing test script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing trajectory_server script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_cart script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_cart_fast script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_cart_fast_max_acc script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_cart_fast_smooth script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_new script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_new_cart script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_trapezoidal script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installed /ws/src/ba-alexanderschaefer/workspace/build/joint_control
running symlink_data
symbolically linking /ws/src/ba-alexanderschaefer/workspace/build/joint_control/resource/joint_control -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/ament_index/resource_index/packages

31
workspace/log/build_2025-04-30_09-37-17/joint_control/stdout_stderr.log Normal file
View File

@@ -0,0 +1,31 @@
running develop
running egg_info
writing joint_control.egg-info/PKG-INFO
writing dependency_links to joint_control.egg-info/dependency_links.txt
writing entry points to joint_control.egg-info/entry_points.txt
writing requirements to joint_control.egg-info/requires.txt
writing top-level names to joint_control.egg-info/top_level.txt
reading manifest file 'joint_control.egg-info/SOURCES.txt'
writing manifest file 'joint_control.egg-info/SOURCES.txt'
running build_ext
Creating /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint-control.egg-link (link to .)
Installing cart_coords script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing joint_control script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing plugdata script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing plugdata_cart script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing plugdata_cart_fix script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing plugdata_cart_smooth script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing test script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing trajectory_server script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_cart script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_cart_fast script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_cart_fast_max_acc script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_cart_fast_smooth script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_new script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_new_cart script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_trapezoidal script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
Installed /ws/src/ba-alexanderschaefer/workspace/build/joint_control
running symlink_data
symbolically linking /ws/src/ba-alexanderschaefer/workspace/build/joint_control/resource/joint_control -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/ament_index/resource_index/packages
error: [Errno 17] File exists: '/ws/src/ba-alexanderschaefer/workspace/build/joint_control/resource/joint_control' -> '/ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/ament_index/resource_index/packages/joint_control'

33
workspace/log/build_2025-04-30_09-37-17/joint_control/streams.log Normal file
View File

@@ -0,0 +1,33 @@
[0.334s] Invoking command in '/ws/src/ba-alexanderschaefer/workspace/build/joint_control': PYTHONPATH=/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py develop --editable --build-directory /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build --no-deps symlink_data
[0.472s] running develop
[0.550s] running egg_info
[0.550s] writing joint_control.egg-info/PKG-INFO
[0.550s] writing dependency_links to joint_control.egg-info/dependency_links.txt
[0.550s] writing entry points to joint_control.egg-info/entry_points.txt
[0.550s] writing requirements to joint_control.egg-info/requires.txt
[0.550s] writing top-level names to joint_control.egg-info/top_level.txt
[0.551s] reading manifest file 'joint_control.egg-info/SOURCES.txt'
[0.552s] writing manifest file 'joint_control.egg-info/SOURCES.txt'
[0.552s] running build_ext
[0.552s] Creating /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint-control.egg-link (link to .)
[0.553s] Installing cart_coords script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
[0.553s] Installing joint_control script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
[0.553s] Installing plugdata script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
[0.553s] Installing plugdata_cart script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
[0.553s] Installing plugdata_cart_fix script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
[0.553s] Installing plugdata_cart_smooth script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
[0.553s] Installing test script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
[0.553s] Installing trajectory_server script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
[0.554s] Installing trajectory_server_cart script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
[0.554s] Installing trajectory_server_cart_fast script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
[0.554s] Installing trajectory_server_cart_fast_max_acc script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
[0.554s] Installing trajectory_server_cart_fast_smooth script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
[0.554s] Installing trajectory_server_new script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
[0.554s] Installing trajectory_server_new_cart script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
[0.554s] Installing trajectory_server_trapezoidal script to /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/joint_control
[0.554s]
[0.554s] Installed /ws/src/ba-alexanderschaefer/workspace/build/joint_control
[0.554s] running symlink_data
[0.554s] symbolically linking /ws/src/ba-alexanderschaefer/workspace/build/joint_control/resource/joint_control -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/ament_index/resource_index/packages
[0.554s] error: [Errno 17] File exists: '/ws/src/ba-alexanderschaefer/workspace/build/joint_control/resource/joint_control' -> '/ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/ament_index/resource_index/packages/joint_control'
[0.567s] Invoked command in '/ws/src/ba-alexanderschaefer/workspace/build/joint_control' returned '1': PYTHONPATH=/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py develop --editable --build-directory /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build --no-deps symlink_data

141
workspace/log/build_2025-04-30_09-37-17/logger_all.log Normal file
View File

@@ -0,0 +1,141 @@
[0.064s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build', '--packages-select', 'joint_control', '--symlink-install']
[0.064s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='build', build_base='build', install_base='install', merge_install=False, symlink_install=True, test_result_base=None, continue_on_error=False, executor='parallel', parallel_workers=16, event_handlers=None, ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['.'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=['joint_control'], packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, cmake_args=None, cmake_target=None, cmake_target_skip_unavailable=False, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, ament_cmake_args=None, catkin_cmake_args=None, catkin_skip_building_tests=False, mixin_files=None, mixin=None, verb_parser=<colcon_mixin.mixin.mixin_argument.MixinArgumentDecorator object at 0x7f6741194eb0>, verb_extension=<colcon_core.verb.build.BuildVerb object at 0x7f67412824d0>, main=<bound method BuildVerb.main of <colcon_core.verb.build.BuildVerb object at 0x7f67412824d0>>, mixin_verb=('build',))
[0.158s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters
[0.158s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/Gazebo.meta'
[0.159s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/fastrtps.meta'
[0.159s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters
[0.159s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters
[0.159s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters
[0.159s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover
[0.159s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover
[0.159s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/ws/src/ba-alexanderschaefer/workspace'
[0.159s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install']
[0.159s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore'
[0.159s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install'
[0.159s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg']
[0.159s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg'
[0.159s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta']
[0.159s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta'
[0.159s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros']
[0.159s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros'
[0.172s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python']
[0.172s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake'
[0.172s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python'
[0.172s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py']
[0.172s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py'
[0.172s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install']
[0.172s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore'
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install']
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore'
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install']
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore'
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ignore', 'ignore_ament_install']
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore'
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore_ament_install'
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_pkg']
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_pkg'
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_meta']
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_meta'
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ros']
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ros'
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['cmake', 'python']
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'cmake'
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python'
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['python_setup_py']
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python_setup_py'
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['ignore', 'ignore_ament_install']
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'ignore'
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'ignore_ament_install'
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['colcon_pkg']
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'colcon_pkg'
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['colcon_meta']
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'colcon_meta'
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['ros']
[0.173s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'ros'
[0.175s] DEBUG:colcon.colcon_core.package_identification:Package 'src/joint_control' with type 'ros.ament_python' and name 'joint_control'
[0.175s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['ignore', 'ignore_ament_install']
[0.175s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'ignore'
[0.175s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'ignore_ament_install'
[0.175s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['colcon_pkg']
[0.175s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'colcon_pkg'
[0.175s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['colcon_meta']
[0.175s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'colcon_meta'
[0.175s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['ros']
[0.175s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'ros'
[0.175s] DEBUG:colcon.colcon_core.package_identification:Package 'src/joint_info' with type 'ros.ament_python' and name 'joint_info'
[0.175s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['ignore', 'ignore_ament_install']
[0.176s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'ignore'
[0.176s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'ignore_ament_install'
[0.176s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['colcon_pkg']
[0.176s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'colcon_pkg'
[0.176s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['colcon_meta']
[0.176s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'colcon_meta'
[0.176s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['ros']
[0.176s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'ros'
[0.176s] DEBUG:colcon.colcon_core.package_identification:Package 'src/mock_robot' with type 'ros.ament_python' and name 'mock_robot'
[0.176s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['ignore', 'ignore_ament_install']
[0.176s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'ignore'
[0.176s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'ignore_ament_install'
[0.176s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['colcon_pkg']
[0.176s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'colcon_pkg'
[0.176s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['colcon_meta']
[0.176s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'colcon_meta'
[0.176s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['ros']
[0.176s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'ros'
[0.176s] DEBUG:colcon.colcon_core.package_identification:Package 'src/painting_robot_control' with type 'ros.ament_python' and name 'painting_robot_control'
[0.176s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults
[0.177s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover
[0.177s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults
[0.177s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover
[0.177s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults
[0.192s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'joint_info' in 'src/joint_info'
[0.192s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'mock_robot' in 'src/mock_robot'
[0.192s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'painting_robot_control' in 'src/painting_robot_control'
[0.192s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'cmake_args' from command line to 'None'
[0.192s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'cmake_target' from command line to 'None'
[0.192s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'cmake_target_skip_unavailable' from command line to 'False'
[0.192s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'cmake_clean_cache' from command line to 'False'
[0.192s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'cmake_clean_first' from command line to 'False'
[0.192s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'cmake_force_configure' from command line to 'False'
[0.192s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'ament_cmake_args' from command line to 'None'
[0.192s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'catkin_cmake_args' from command line to 'None'
[0.192s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'catkin_skip_building_tests' from command line to 'False'
[0.192s] DEBUG:colcon.colcon_core.verb:Building package 'joint_control' with the following arguments: {'ament_cmake_args': None, 'build_base': '/ws/src/ba-alexanderschaefer/workspace/build/joint_control', 'catkin_cmake_args': None, 'catkin_skip_building_tests': False, 'cmake_args': None, 'cmake_clean_cache': False, 'cmake_clean_first': False, 'cmake_force_configure': False, 'cmake_target': None, 'cmake_target_skip_unavailable': False, 'install_base': '/ws/src/ba-alexanderschaefer/workspace/install/joint_control', 'merge_install': False, 'path': '/ws/src/ba-alexanderschaefer/workspace/src/joint_control', 'symlink_install': True, 'test_result_base': None}
[0.192s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor
[0.193s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete
[0.193s] INFO:colcon.colcon_ros.task.ament_python.build:Building ROS package in '/ws/src/ba-alexanderschaefer/workspace/src/joint_control' with build type 'ament_python'
[0.193s] Level 1:colcon.colcon_core.shell:create_environment_hook('joint_control', 'ament_prefix_path')
[0.195s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems
[0.195s] INFO:colcon.colcon_core.shell:Creating environment hook '/ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/joint_control/hook/ament_prefix_path.ps1'
[0.195s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/joint_control/hook/ament_prefix_path.dsv'
[0.196s] INFO:colcon.colcon_core.shell:Creating environment hook '/ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/joint_control/hook/ament_prefix_path.sh'
[0.196s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell
[0.196s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment
[0.333s] INFO:colcon.colcon_core.task.python.build:Building Python package in '/ws/src/ba-alexanderschaefer/workspace/src/joint_control'
[0.333s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell
[0.333s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment
[0.528s] DEBUG:colcon.colcon_core.event_handler.log_command:Invoking command in '/ws/src/ba-alexanderschaefer/workspace/build/joint_control': PYTHONPATH=/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py develop --editable --build-directory /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build --no-deps symlink_data
[0.760s] DEBUG:colcon.colcon_core.event_handler.log_command:Invoked command in '/ws/src/ba-alexanderschaefer/workspace/build/joint_control' returned '1': PYTHONPATH=/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py develop --editable --build-directory /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build --no-deps symlink_data
[0.770s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop
[0.770s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed
[0.770s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '1'
[0.771s] DEBUG:colcon.colcon_core.event_reactor:joining thread
[0.774s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.notify_send': Could not find 'notify-send'
[0.774s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems
[0.774s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems
[0.774s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2'
[0.774s] DEBUG:colcon.colcon_notification.desktop_notification.notify2:Failed to initialize notify2: org.freedesktop.DBus.Error.NotSupported: Unable to autolaunch a dbus-daemon without a $DISPLAY for X11
[0.774s] DEBUG:colcon.colcon_core.event_reactor:joined thread
[0.774s] INFO:colcon.colcon_core.shell:Creating prefix script '/ws/src/ba-alexanderschaefer/workspace/install/local_setup.ps1'
[0.775s] INFO:colcon.colcon_core.shell:Creating prefix util module '/ws/src/ba-alexanderschaefer/workspace/install/_local_setup_util_ps1.py'
[0.776s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/ws/src/ba-alexanderschaefer/workspace/install/setup.ps1'
[0.776s] INFO:colcon.colcon_core.shell:Creating prefix script '/ws/src/ba-alexanderschaefer/workspace/install/local_setup.sh'
[0.776s] INFO:colcon.colcon_core.shell:Creating prefix util module '/ws/src/ba-alexanderschaefer/workspace/install/_local_setup_util_sh.py'
[0.777s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/ws/src/ba-alexanderschaefer/workspace/install/setup.sh'
[0.777s] INFO:colcon.colcon_core.shell:Creating prefix script '/ws/src/ba-alexanderschaefer/workspace/install/local_setup.bash'
[0.777s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/ws/src/ba-alexanderschaefer/workspace/install/setup.bash'
[0.778s] INFO:colcon.colcon_core.shell:Creating prefix script '/ws/src/ba-alexanderschaefer/workspace/install/local_setup.zsh'
[0.778s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/ws/src/ba-alexanderschaefer/workspace/install/setup.zsh'

79
workspace/log/build_2025-04-30_09-37-26/events.log Normal file
View File

@@ -0,0 +1,79 @@
[0.000000] (-) TimerEvent: {}
[0.000112] (-) JobUnselected: {'identifier': 'joint_info'}
[0.000184] (-) JobUnselected: {'identifier': 'mock_robot'}
[0.000344] (-) JobUnselected: {'identifier': 'painting_robot_control'}
[0.000414] (joint_control) JobQueued: {'identifier': 'joint_control', 'dependencies': OrderedDict()}
[0.000467] (joint_control) JobStarted: {'identifier': 'joint_control'}
[0.099936] (-) TimerEvent: {}
[0.200136] (-) TimerEvent: {}
[0.300332] (-) TimerEvent: {}
[0.325584] (joint_control) Command: {'cmd': ['/usr/bin/python3', '-W', 'ignore:setup.py install is deprecated', '-W', 'ignore:easy_install command is deprecated', 'setup.py', 'develop', '--uninstall', '--editable', '--build-directory', '/ws/src/ba-alexanderschaefer/workspace/build/joint_control/build'], 'cwd': '/ws/src/ba-alexanderschaefer/workspace/build/joint_control', 'env': {'HOSTNAME': 'hapticslab2', 'SHLVL': '1', 'LD_LIBRARY_PATH': '/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib', 'HOME': '/root', 'OLDPWD': '/ws/src/ba-alexanderschaefer', 'ROS_PYTHON_VERSION': '3', 'ROS_DISTRO': 'humble', '_': '/usr/bin/colcon', 'ROS_VERSION': '2', 'TERM': 'xterm', 'ROS_LOCALHOST_ONLY': '0', 'PATH': '/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin', 'LANG': 'C.UTF-8', '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:', 'AMENT_PREFIX_PATH': '/opt/ros/humble', 'PWD': '/ws/src/ba-alexanderschaefer/workspace/build/joint_control', 'LC_ALL': 'C.UTF-8', 'PYTHONPATH': '/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/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', 'COLCON': '1'}, 'shell': False}
[0.400610] (-) TimerEvent: {}
[0.460411] (joint_control) StdoutLine: {'line': b'running develop\n'}
[0.500685] (-) TimerEvent: {}
[0.532717] (joint_control) StdoutLine: {'line': b'Removing /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint-control.egg-link (link to .)\n'}
[0.545079] (joint_control) CommandEnded: {'returncode': 0}
[0.545712] (joint_control) Command: {'cmd': ['/usr/bin/python3', '-W', 'ignore:setup.py install is deprecated', '-W', 'ignore:easy_install command is deprecated', 'setup.py', 'egg_info', '--egg-base', '../../build/joint_control', 'build', '--build-base', '/ws/src/ba-alexanderschaefer/workspace/build/joint_control/build', 'install', '--record', '/ws/src/ba-alexanderschaefer/workspace/build/joint_control/install.log', '--single-version-externally-managed', 'install_data', '--force'], 'cwd': '/ws/src/ba-alexanderschaefer/workspace/src/joint_control', 'env': {'HOSTNAME': 'hapticslab2', 'SHLVL': '1', 'LD_LIBRARY_PATH': '/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib', 'HOME': '/root', 'OLDPWD': '/ws/src/ba-alexanderschaefer', 'ROS_PYTHON_VERSION': '3', 'ROS_DISTRO': 'humble', '_': '/usr/bin/colcon', 'ROS_VERSION': '2', 'TERM': 'xterm', 'ROS_LOCALHOST_ONLY': '0', 'PATH': '/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin', 'LANG': 'C.UTF-8', '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:', 'AMENT_PREFIX_PATH': '/opt/ros/humble', 'PWD': '/ws/src/ba-alexanderschaefer/workspace/build/joint_control', 'LC_ALL': 'C.UTF-8', 'PYTHONPATH': '/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/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', 'COLCON': '1'}, 'shell': False}
[0.600759] (-) TimerEvent: {}
[0.655665] (joint_control) StdoutLine: {'line': b'running egg_info\n'}
[0.655984] (joint_control) StdoutLine: {'line': b'writing ../../build/joint_control/joint_control.egg-info/PKG-INFO\n'}
[0.656084] (joint_control) StdoutLine: {'line': b'writing dependency_links to ../../build/joint_control/joint_control.egg-info/dependency_links.txt\n'}
[0.656198] (joint_control) StdoutLine: {'line': b'writing entry points to ../../build/joint_control/joint_control.egg-info/entry_points.txt\n'}
[0.656269] (joint_control) StdoutLine: {'line': b'writing requirements to ../../build/joint_control/joint_control.egg-info/requires.txt\n'}
[0.656333] (joint_control) StdoutLine: {'line': b'writing top-level names to ../../build/joint_control/joint_control.egg-info/top_level.txt\n'}
[0.657140] (joint_control) StdoutLine: {'line': b"reading manifest file '../../build/joint_control/joint_control.egg-info/SOURCES.txt'\n"}
[0.657672] (joint_control) StdoutLine: {'line': b"writing manifest file '../../build/joint_control/joint_control.egg-info/SOURCES.txt'\n"}
[0.657734] (joint_control) StdoutLine: {'line': b'running build\n'}
[0.657784] (joint_control) StdoutLine: {'line': b'running build_py\n'}
[0.657927] (joint_control) StdoutLine: {'line': b'copying joint_control/plugdata_cart.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control\n'}
[0.658019] (joint_control) StdoutLine: {'line': b'copying joint_control/plugdata_cart_smooth.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control\n'}
[0.658096] (joint_control) StdoutLine: {'line': b'copying joint_control/test.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control\n'}
[0.658211] (joint_control) StdoutLine: {'line': b'copying joint_control/trajectory_server_cart_fast.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control\n'}
[0.658278] (joint_control) StdoutLine: {'line': b'copying joint_control/trajectory_server_cart_fast_max_acc.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control\n'}
[0.658348] (joint_control) StdoutLine: {'line': b'copying joint_control/trajectory_server_cart_fast_smooth.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control\n'}
[0.658414] (joint_control) StdoutLine: {'line': b'copying joint_control/trajectory_server_trapezoidal.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control\n'}
[0.658482] (joint_control) StdoutLine: {'line': b'copying joint_control/plugdata.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control\n'}
[0.658550] (joint_control) StdoutLine: {'line': b'copying joint_control/plugdata_cart_fix.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control\n'}
[0.658620] (joint_control) StdoutLine: {'line': b'copying joint_control/trajectory_server_new.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control\n'}
[0.658690] (joint_control) StdoutLine: {'line': b'copying joint_control/trajectory_server_new_cart.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control\n'}
[0.658877] (joint_control) StdoutLine: {'line': b'running install\n'}
[0.658998] (joint_control) StdoutLine: {'line': b'running install_lib\n'}
[0.659315] (joint_control) StdoutLine: {'line': b'creating /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control\n'}
[0.659361] (joint_control) StdoutLine: {'line': b'copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/__init__.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control\n'}
[0.659417] (joint_control) StdoutLine: {'line': b'copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/cart_tcp_server.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control\n'}
[0.659484] (joint_control) StdoutLine: {'line': b'copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/joint_angles_server.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control\n'}
[0.659552] (joint_control) StdoutLine: {'line': b'copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control\n'}
[0.659619] (joint_control) StdoutLine: {'line': b'copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control\n'}
[0.659686] (joint_control) StdoutLine: {'line': b'copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/plugdata_cart.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control\n'}
[0.659753] (joint_control) StdoutLine: {'line': b'copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/plugdata_cart_smooth.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control\n'}
[0.659821] (joint_control) StdoutLine: {'line': b'copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/test.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control\n'}
[0.659888] (joint_control) StdoutLine: {'line': b'copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control\n'}
[0.659959] (joint_control) StdoutLine: {'line': b'copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast_max_acc.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control\n'}
[0.660027] (joint_control) StdoutLine: {'line': b'copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast_smooth.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control\n'}
[0.660093] (joint_control) StdoutLine: {'line': b'copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_trapezoidal.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control\n'}
[0.660163] (joint_control) StdoutLine: {'line': b'copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/plugdata.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control\n'}
[0.660230] (joint_control) StdoutLine: {'line': b'copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/plugdata_cart_fix.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control\n'}
[0.660306] (joint_control) StdoutLine: {'line': b'copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_new.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control\n'}
[0.660375] (joint_control) StdoutLine: {'line': b'copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_new_cart.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control\n'}
[0.660646] (joint_control) StdoutLine: {'line': b'byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__init__.py to __init__.cpython-310.pyc\n'}
[0.660772] (joint_control) StdoutLine: {'line': b'byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/cart_tcp_server.py to cart_tcp_server.cpython-310.pyc\n'}
[0.661229] (joint_control) StdoutLine: {'line': b'byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/joint_angles_server.py to joint_angles_server.cpython-310.pyc\n'}
[0.661565] (joint_control) StdoutLine: {'line': b'byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server.py to trajectory_server.cpython-310.pyc\n'}
[0.661865] (joint_control) StdoutLine: {'line': b'byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart.py to trajectory_server_cart.cpython-310.pyc\n'}
[0.662317] (joint_control) StdoutLine: {'line': b'byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart.py to plugdata_cart.cpython-310.pyc\n'}
[0.662945] (joint_control) StdoutLine: {'line': b'byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart_smooth.py to plugdata_cart_smooth.cpython-310.pyc\n'}
[0.663825] (joint_control) StdoutLine: {'line': b'byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/test.py to test.cpython-310.pyc\n'}
[0.664002] (joint_control) StdoutLine: {'line': b'byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast.py to trajectory_server_cart_fast.cpython-310.pyc\n'}
[0.664542] (joint_control) StdoutLine: {'line': b'byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast_max_acc.py to trajectory_server_cart_fast_max_acc.cpython-310.pyc\n'}
[0.665189] (joint_control) StdoutLine: {'line': b'byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast_smooth.py to trajectory_server_cart_fast_smooth.cpython-310.pyc\n'}
[0.665742] (joint_control) StdoutLine: {'line': b'byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_trapezoidal.py to trajectory_server_trapezoidal.cpython-310.pyc\n'}
[0.666444] (joint_control) StdoutLine: {'line': b'byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata.py to plugdata.cpython-310.pyc\n'}
[0.667091] (joint_control) StdoutLine: {'line': b'byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart_fix.py to plugdata_cart_fix.cpython-310.pyc\n'}
[0.668116] (joint_control) StdoutLine: {'line': b'byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_new.py to trajectory_server_new.cpython-310.pyc\n'}
[0.668737] (joint_control) StdoutLine: {'line': b'byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_new_cart.py to trajectory_server_new_cart.cpython-310.pyc\n'}
[0.669315] (joint_control) StdoutLine: {'line': b'running install_data\n'}
[0.669378] (joint_control) StdoutLine: {'line': b'copying resource/joint_control -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/ament_index/resource_index/packages\n'}
[0.669441] (joint_control) StderrLine: {'line': b"error: could not create '/ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/ament_index/resource_index/packages/joint_control': No such file or directory\n"}
[0.678000] (joint_control) CommandEnded: {'returncode': 1}
[0.678164] (joint_control) JobEnded: {'identifier': 'joint_control', 'rc': 1}
[0.688266] (-) EventReactorShutdown: {}

4
workspace/log/build_2025-04-30_09-37-26/joint_control/command.log Normal file
View File

@@ -0,0 +1,4 @@
Invoking command in '/ws/src/ba-alexanderschaefer/workspace/build/joint_control': PYTHONPATH=/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py develop --uninstall --editable --build-directory /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build
Invoked command in '/ws/src/ba-alexanderschaefer/workspace/build/joint_control' returned '0': PYTHONPATH=/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py develop --uninstall --editable --build-directory /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build
Invoking command in '/ws/src/ba-alexanderschaefer/workspace/src/joint_control': PYTHONPATH=/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/joint_control build --build-base /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build install --record /ws/src/ba-alexanderschaefer/workspace/build/joint_control/install.log --single-version-externally-managed install_data --force
Invoked command in '/ws/src/ba-alexanderschaefer/workspace/src/joint_control' returned '1': PYTHONPATH=/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/joint_control build --build-base /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build install --record /ws/src/ba-alexanderschaefer/workspace/build/joint_control/install.log --single-version-externally-managed install_data --force

1
workspace/log/build_2025-04-30_09-37-26/joint_control/stderr.log Normal file
View File

@@ -0,0 +1 @@
error: could not create '/ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/ament_index/resource_index/packages/joint_control': No such file or directory

60
workspace/log/build_2025-04-30_09-37-26/joint_control/stdout.log Normal file
View File

@@ -0,0 +1,60 @@
running develop
Removing /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint-control.egg-link (link to .)
running egg_info
writing ../../build/joint_control/joint_control.egg-info/PKG-INFO
writing dependency_links to ../../build/joint_control/joint_control.egg-info/dependency_links.txt
writing entry points to ../../build/joint_control/joint_control.egg-info/entry_points.txt
writing requirements to ../../build/joint_control/joint_control.egg-info/requires.txt
writing top-level names to ../../build/joint_control/joint_control.egg-info/top_level.txt
reading manifest file '../../build/joint_control/joint_control.egg-info/SOURCES.txt'
writing manifest file '../../build/joint_control/joint_control.egg-info/SOURCES.txt'
running build
running build_py
copying joint_control/plugdata_cart.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/plugdata_cart_smooth.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/test.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/trajectory_server_cart_fast.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/trajectory_server_cart_fast_max_acc.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/trajectory_server_cart_fast_smooth.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/trajectory_server_trapezoidal.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/plugdata.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/plugdata_cart_fix.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/trajectory_server_new.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/trajectory_server_new_cart.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
running install
running install_lib
creating /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/__init__.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/cart_tcp_server.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/joint_angles_server.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/plugdata_cart.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/plugdata_cart_smooth.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/test.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast_max_acc.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast_smooth.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_trapezoidal.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/plugdata.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/plugdata_cart_fix.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_new.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_new_cart.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__init__.py to __init__.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/cart_tcp_server.py to cart_tcp_server.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/joint_angles_server.py to joint_angles_server.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server.py to trajectory_server.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart.py to trajectory_server_cart.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart.py to plugdata_cart.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart_smooth.py to plugdata_cart_smooth.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/test.py to test.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast.py to trajectory_server_cart_fast.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast_max_acc.py to trajectory_server_cart_fast_max_acc.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast_smooth.py to trajectory_server_cart_fast_smooth.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_trapezoidal.py to trajectory_server_trapezoidal.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata.py to plugdata.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart_fix.py to plugdata_cart_fix.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_new.py to trajectory_server_new.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_new_cart.py to trajectory_server_new_cart.cpython-310.pyc
running install_data
copying resource/joint_control -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/ament_index/resource_index/packages

61
workspace/log/build_2025-04-30_09-37-26/joint_control/stdout_stderr.log Normal file
View File

@@ -0,0 +1,61 @@
running develop
Removing /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint-control.egg-link (link to .)
running egg_info
writing ../../build/joint_control/joint_control.egg-info/PKG-INFO
writing dependency_links to ../../build/joint_control/joint_control.egg-info/dependency_links.txt
writing entry points to ../../build/joint_control/joint_control.egg-info/entry_points.txt
writing requirements to ../../build/joint_control/joint_control.egg-info/requires.txt
writing top-level names to ../../build/joint_control/joint_control.egg-info/top_level.txt
reading manifest file '../../build/joint_control/joint_control.egg-info/SOURCES.txt'
writing manifest file '../../build/joint_control/joint_control.egg-info/SOURCES.txt'
running build
running build_py
copying joint_control/plugdata_cart.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/plugdata_cart_smooth.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/test.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/trajectory_server_cart_fast.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/trajectory_server_cart_fast_max_acc.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/trajectory_server_cart_fast_smooth.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/trajectory_server_trapezoidal.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/plugdata.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/plugdata_cart_fix.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/trajectory_server_new.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
copying joint_control/trajectory_server_new_cart.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
running install
running install_lib
creating /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/__init__.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/cart_tcp_server.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/joint_angles_server.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/plugdata_cart.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/plugdata_cart_smooth.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/test.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast_max_acc.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast_smooth.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_trapezoidal.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/plugdata.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/plugdata_cart_fix.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_new.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_new_cart.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__init__.py to __init__.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/cart_tcp_server.py to cart_tcp_server.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/joint_angles_server.py to joint_angles_server.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server.py to trajectory_server.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart.py to trajectory_server_cart.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart.py to plugdata_cart.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart_smooth.py to plugdata_cart_smooth.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/test.py to test.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast.py to trajectory_server_cart_fast.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast_max_acc.py to trajectory_server_cart_fast_max_acc.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast_smooth.py to trajectory_server_cart_fast_smooth.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_trapezoidal.py to trajectory_server_trapezoidal.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata.py to plugdata.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart_fix.py to plugdata_cart_fix.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_new.py to trajectory_server_new.cpython-310.pyc
byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_new_cart.py to trajectory_server_new_cart.cpython-310.pyc
running install_data
copying resource/joint_control -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/ament_index/resource_index/packages
error: could not create '/ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/ament_index/resource_index/packages/joint_control': No such file or directory

65
workspace/log/build_2025-04-30_09-37-26/joint_control/streams.log Normal file
View File

@@ -0,0 +1,65 @@
[0.326s] Invoking command in '/ws/src/ba-alexanderschaefer/workspace/build/joint_control': PYTHONPATH=/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py develop --uninstall --editable --build-directory /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build
[0.460s] running develop
[0.532s] Removing /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint-control.egg-link (link to .)
[0.545s] Invoked command in '/ws/src/ba-alexanderschaefer/workspace/build/joint_control' returned '0': PYTHONPATH=/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py develop --uninstall --editable --build-directory /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build
[0.546s] Invoking command in '/ws/src/ba-alexanderschaefer/workspace/src/joint_control': PYTHONPATH=/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/joint_control build --build-base /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build install --record /ws/src/ba-alexanderschaefer/workspace/build/joint_control/install.log --single-version-externally-managed install_data --force
[0.655s] running egg_info
[0.656s] writing ../../build/joint_control/joint_control.egg-info/PKG-INFO
[0.656s] writing dependency_links to ../../build/joint_control/joint_control.egg-info/dependency_links.txt
[0.656s] writing entry points to ../../build/joint_control/joint_control.egg-info/entry_points.txt
[0.656s] writing requirements to ../../build/joint_control/joint_control.egg-info/requires.txt
[0.656s] writing top-level names to ../../build/joint_control/joint_control.egg-info/top_level.txt
[0.657s] reading manifest file '../../build/joint_control/joint_control.egg-info/SOURCES.txt'
[0.657s] writing manifest file '../../build/joint_control/joint_control.egg-info/SOURCES.txt'
[0.657s] running build
[0.657s] running build_py
[0.657s] copying joint_control/plugdata_cart.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
[0.658s] copying joint_control/plugdata_cart_smooth.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
[0.658s] copying joint_control/test.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
[0.658s] copying joint_control/trajectory_server_cart_fast.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
[0.658s] copying joint_control/trajectory_server_cart_fast_max_acc.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
[0.658s] copying joint_control/trajectory_server_cart_fast_smooth.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
[0.658s] copying joint_control/trajectory_server_trapezoidal.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
[0.658s] copying joint_control/plugdata.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
[0.658s] copying joint_control/plugdata_cart_fix.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
[0.658s] copying joint_control/trajectory_server_new.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
[0.658s] copying joint_control/trajectory_server_new_cart.py -> /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control
[0.658s] running install
[0.659s] running install_lib
[0.659s] creating /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
[0.659s] copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/__init__.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
[0.659s] copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/cart_tcp_server.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
[0.659s] copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/joint_angles_server.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
[0.659s] copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
[0.659s] copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
[0.659s] copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/plugdata_cart.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
[0.659s] copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/plugdata_cart_smooth.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
[0.659s] copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/test.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
[0.659s] copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
[0.660s] copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast_max_acc.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
[0.660s] copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_cart_fast_smooth.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
[0.660s] copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_trapezoidal.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
[0.660s] copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/plugdata.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
[0.660s] copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/plugdata_cart_fix.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
[0.660s] copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_new.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
[0.660s] copying /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build/lib/joint_control/trajectory_server_new_cart.py -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control
[0.660s] byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/__init__.py to __init__.cpython-310.pyc
[0.660s] byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/cart_tcp_server.py to cart_tcp_server.cpython-310.pyc
[0.661s] byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/joint_angles_server.py to joint_angles_server.cpython-310.pyc
[0.661s] byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server.py to trajectory_server.cpython-310.pyc
[0.661s] byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart.py to trajectory_server_cart.cpython-310.pyc
[0.662s] byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart.py to plugdata_cart.cpython-310.pyc
[0.662s] byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart_smooth.py to plugdata_cart_smooth.cpython-310.pyc
[0.663s] byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/test.py to test.cpython-310.pyc
[0.664s] byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast.py to trajectory_server_cart_fast.cpython-310.pyc
[0.664s] byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast_max_acc.py to trajectory_server_cart_fast_max_acc.cpython-310.pyc
[0.665s] byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_cart_fast_smooth.py to trajectory_server_cart_fast_smooth.cpython-310.pyc
[0.665s] byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_trapezoidal.py to trajectory_server_trapezoidal.cpython-310.pyc
[0.666s] byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata.py to plugdata.cpython-310.pyc
[0.667s] byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/plugdata_cart_fix.py to plugdata_cart_fix.cpython-310.pyc
[0.668s] byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_new.py to trajectory_server_new.cpython-310.pyc
[0.668s] byte-compiling /ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages/joint_control/trajectory_server_new_cart.py to trajectory_server_new_cart.cpython-310.pyc
[0.669s] running install_data
[0.669s] copying resource/joint_control -> /ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/ament_index/resource_index/packages
[0.669s] error: could not create '/ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/ament_index/resource_index/packages/joint_control': No such file or directory
[0.678s] Invoked command in '/ws/src/ba-alexanderschaefer/workspace/src/joint_control' returned '1': PYTHONPATH=/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/joint_control build --build-base /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build install --record /ws/src/ba-alexanderschaefer/workspace/build/joint_control/install.log --single-version-externally-managed install_data --force

143
workspace/log/build_2025-04-30_09-37-26/logger_all.log Normal file
View File

@@ -0,0 +1,143 @@
[0.063s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build', '--packages-select', 'joint_control']
[0.063s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='build', build_base='build', install_base='install', merge_install=False, symlink_install=False, test_result_base=None, continue_on_error=False, executor='parallel', parallel_workers=16, event_handlers=None, ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['.'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=['joint_control'], packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, cmake_args=None, cmake_target=None, cmake_target_skip_unavailable=False, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, ament_cmake_args=None, catkin_cmake_args=None, catkin_skip_building_tests=False, mixin_files=None, mixin=None, verb_parser=<colcon_mixin.mixin.mixin_argument.MixinArgumentDecorator object at 0x7f7fe8188e80>, verb_extension=<colcon_core.verb.build.BuildVerb object at 0x7f7fe82764a0>, main=<bound method BuildVerb.main of <colcon_core.verb.build.BuildVerb object at 0x7f7fe82764a0>>, mixin_verb=('build',))
[0.150s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters
[0.151s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/Gazebo.meta'
[0.151s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/fastrtps.meta'
[0.151s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters
[0.151s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters
[0.151s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters
[0.151s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover
[0.151s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover
[0.151s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/ws/src/ba-alexanderschaefer/workspace'
[0.151s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install']
[0.151s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore'
[0.151s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install'
[0.151s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg']
[0.151s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg'
[0.151s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta']
[0.151s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta'
[0.151s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros']
[0.151s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros'
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python']
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake'
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python'
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py']
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py'
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install']
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore'
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install']
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore'
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install']
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore'
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ignore', 'ignore_ament_install']
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore'
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore_ament_install'
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_pkg']
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_pkg'
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_meta']
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_meta'
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ros']
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ros'
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['cmake', 'python']
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'cmake'
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python'
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['python_setup_py']
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python_setup_py'
[0.165s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['ignore', 'ignore_ament_install']
[0.166s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'ignore'
[0.166s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'ignore_ament_install'
[0.166s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['colcon_pkg']
[0.166s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'colcon_pkg'
[0.166s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['colcon_meta']
[0.166s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'colcon_meta'
[0.166s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['ros']
[0.166s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'ros'
[0.167s] DEBUG:colcon.colcon_core.package_identification:Package 'src/joint_control' with type 'ros.ament_python' and name 'joint_control'
[0.167s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['ignore', 'ignore_ament_install']
[0.167s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'ignore'
[0.167s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'ignore_ament_install'
[0.167s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['colcon_pkg']
[0.167s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'colcon_pkg'
[0.167s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['colcon_meta']
[0.167s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'colcon_meta'
[0.167s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['ros']
[0.167s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'ros'
[0.168s] DEBUG:colcon.colcon_core.package_identification:Package 'src/joint_info' with type 'ros.ament_python' and name 'joint_info'
[0.168s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['ignore', 'ignore_ament_install']
[0.168s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'ignore'
[0.168s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'ignore_ament_install'
[0.168s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['colcon_pkg']
[0.168s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'colcon_pkg'
[0.168s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['colcon_meta']
[0.168s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'colcon_meta'
[0.168s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['ros']
[0.168s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'ros'
[0.168s] DEBUG:colcon.colcon_core.package_identification:Package 'src/mock_robot' with type 'ros.ament_python' and name 'mock_robot'
[0.168s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['ignore', 'ignore_ament_install']
[0.168s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'ignore'
[0.168s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'ignore_ament_install'
[0.168s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['colcon_pkg']
[0.168s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'colcon_pkg'
[0.168s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['colcon_meta']
[0.168s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'colcon_meta'
[0.168s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['ros']
[0.168s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'ros'
[0.169s] DEBUG:colcon.colcon_core.package_identification:Package 'src/painting_robot_control' with type 'ros.ament_python' and name 'painting_robot_control'
[0.169s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults
[0.169s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover
[0.169s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults
[0.169s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover
[0.169s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults
[0.184s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'joint_info' in 'src/joint_info'
[0.184s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'mock_robot' in 'src/mock_robot'
[0.184s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'painting_robot_control' in 'src/painting_robot_control'
[0.185s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'cmake_args' from command line to 'None'
[0.185s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'cmake_target' from command line to 'None'
[0.185s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'cmake_target_skip_unavailable' from command line to 'False'
[0.185s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'cmake_clean_cache' from command line to 'False'
[0.185s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'cmake_clean_first' from command line to 'False'
[0.185s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'cmake_force_configure' from command line to 'False'
[0.185s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'ament_cmake_args' from command line to 'None'
[0.185s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'catkin_cmake_args' from command line to 'None'
[0.185s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'catkin_skip_building_tests' from command line to 'False'
[0.185s] DEBUG:colcon.colcon_core.verb:Building package 'joint_control' with the following arguments: {'ament_cmake_args': None, 'build_base': '/ws/src/ba-alexanderschaefer/workspace/build/joint_control', 'catkin_cmake_args': None, 'catkin_skip_building_tests': False, 'cmake_args': None, 'cmake_clean_cache': False, 'cmake_clean_first': False, 'cmake_force_configure': False, 'cmake_target': None, 'cmake_target_skip_unavailable': False, 'install_base': '/ws/src/ba-alexanderschaefer/workspace/install/joint_control', 'merge_install': False, 'path': '/ws/src/ba-alexanderschaefer/workspace/src/joint_control', 'symlink_install': False, 'test_result_base': None}
[0.185s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor
[0.185s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete
[0.186s] INFO:colcon.colcon_ros.task.ament_python.build:Building ROS package in '/ws/src/ba-alexanderschaefer/workspace/src/joint_control' with build type 'ament_python'
[0.186s] Level 1:colcon.colcon_core.shell:create_environment_hook('joint_control', 'ament_prefix_path')
[0.187s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems
[0.187s] INFO:colcon.colcon_core.shell:Creating environment hook '/ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/joint_control/hook/ament_prefix_path.ps1'
[0.187s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/joint_control/hook/ament_prefix_path.dsv'
[0.188s] INFO:colcon.colcon_core.shell:Creating environment hook '/ws/src/ba-alexanderschaefer/workspace/install/joint_control/share/joint_control/hook/ament_prefix_path.sh'
[0.188s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell
[0.188s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment
[0.323s] INFO:colcon.colcon_core.task.python.build:Building Python package in '/ws/src/ba-alexanderschaefer/workspace/src/joint_control'
[0.323s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell
[0.323s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment
[0.511s] DEBUG:colcon.colcon_core.event_handler.log_command:Invoking command in '/ws/src/ba-alexanderschaefer/workspace/build/joint_control': PYTHONPATH=/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py develop --uninstall --editable --build-directory /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build
[0.731s] DEBUG:colcon.colcon_core.event_handler.log_command:Invoked command in '/ws/src/ba-alexanderschaefer/workspace/build/joint_control' returned '0': PYTHONPATH=/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py develop --uninstall --editable --build-directory /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build
[0.731s] DEBUG:colcon.colcon_core.event_handler.log_command:Invoking command in '/ws/src/ba-alexanderschaefer/workspace/src/joint_control': PYTHONPATH=/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/joint_control build --build-base /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build install --record /ws/src/ba-alexanderschaefer/workspace/build/joint_control/install.log --single-version-externally-managed install_data --force
[0.863s] DEBUG:colcon.colcon_core.event_handler.log_command:Invoked command in '/ws/src/ba-alexanderschaefer/workspace/src/joint_control' returned '1': PYTHONPATH=/ws/src/ba-alexanderschaefer/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/ws/src/ba-alexanderschaefer/workspace/install/joint_control/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/joint_control build --build-base /ws/src/ba-alexanderschaefer/workspace/build/joint_control/build install --record /ws/src/ba-alexanderschaefer/workspace/build/joint_control/install.log --single-version-externally-managed install_data --force
[0.873s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop
[0.873s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed
[0.873s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '1'
[0.874s] DEBUG:colcon.colcon_core.event_reactor:joining thread
[0.876s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.notify_send': Could not find 'notify-send'
[0.877s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems
[0.877s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems
[0.877s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2'
[0.877s] DEBUG:colcon.colcon_notification.desktop_notification.notify2:Failed to initialize notify2: org.freedesktop.DBus.Error.NotSupported: Unable to autolaunch a dbus-daemon without a $DISPLAY for X11
[0.877s] DEBUG:colcon.colcon_core.event_reactor:joined thread
[0.877s] INFO:colcon.colcon_core.shell:Creating prefix script '/ws/src/ba-alexanderschaefer/workspace/install/local_setup.ps1'
[0.877s] INFO:colcon.colcon_core.shell:Creating prefix util module '/ws/src/ba-alexanderschaefer/workspace/install/_local_setup_util_ps1.py'
[0.878s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/ws/src/ba-alexanderschaefer/workspace/install/setup.ps1'
[0.879s] INFO:colcon.colcon_core.shell:Creating prefix script '/ws/src/ba-alexanderschaefer/workspace/install/local_setup.sh'
[0.879s] INFO:colcon.colcon_core.shell:Creating prefix util module '/ws/src/ba-alexanderschaefer/workspace/install/_local_setup_util_sh.py'
[0.879s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/ws/src/ba-alexanderschaefer/workspace/install/setup.sh'
[0.880s] INFO:colcon.colcon_core.shell:Creating prefix script '/ws/src/ba-alexanderschaefer/workspace/install/local_setup.bash'
[0.880s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/ws/src/ba-alexanderschaefer/workspace/install/setup.bash'
[0.880s] INFO:colcon.colcon_core.shell:Creating prefix script '/ws/src/ba-alexanderschaefer/workspace/install/local_setup.zsh'
[0.881s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/ws/src/ba-alexanderschaefer/workspace/install/setup.zsh'

2
workspace/log/latest_build
View File

@@ -1 +1 @@
build_2025-04-20_11-18-39
build_2025-04-30_09-37-26

2
workspace/src/joint_angles.py
View File

@@ -8,7 +8,7 @@ def main():
osc_startup()
# Make client channels to send packets
osc_udp_client("172.18.0.3", 8000, "osc_client")
osc_udp_client("localhost", 8000, "osc_client")
# Example joint positions to send
joint_positions1 = [0.0,0.0, 0.0, 0.0, 0.0, 0.0]

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

Binary file not shown.

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

Binary file not shown.

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

Binary file not shown.

1
workspace/src/joint_control/joint_control/plugdata_cart_fix.py
View File

@@ -217,6 +217,7 @@ def main():
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