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

AS: commit before final presentation

Browse Source
This commit is contained in:
Alexander Schaefer
2025-05-26 23:38:12 +02:00
parent 26e6abd14f
commit 09644709f9
68 changed files with 69 additions and 4271 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

Binary file not shown.

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

@@ -72,7 +72,7 @@ class OSC_ROS2_interface(Node):
if robot:
while True:
print('+-' * 50)
set_limits = input("Do you want to set limit for x, y and z? (y/n): ").strip().lower()
set_limits = input("Do you want to set limits for x, y and z? (y/n): ").strip().lower()
if set_limits == 'y':
while True:
try:
@@ -96,7 +96,7 @@ class OSC_ROS2_interface(Node):
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()
confirm = input("Do you want your TCP to move in this range? (y/n): ").strip().lower()
if confirm == 'y':
break
elif confirm == 'n':
@@ -114,8 +114,56 @@ class OSC_ROS2_interface(Node):
self.z_limits = [None, None]
break
print("Invalid input. Please enter 'y' or 'n'.")
while True:
print('+-' * 50)
set_limits = input("Do you want to set workspace limits in x, y and z direction? (y/n): ").strip().lower()
if set_limits == 'y':
while True:
try:
self.x_limits_workspace = [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_workspace = [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_workspace = [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_workspace) != 2 or len(self.y_limits_workspace) != 2 or len(self.z_limits_workspace) != 2:
print("Invalid input. Please enter exactly two values (or leave blank) for each limit.")
continue
if (self.x_limits_workspace[0] is not None and self.x_limits_workspace[1] is not None and self.x_limits_workspace[0] >= self.x_limits_workspace[1]) or \
(self.y_limits_workspace[0] is not None and self.y_limits_workspace[1] is not None and self.y_limits_workspace[0] >= self.y_limits_workspace[1]) or \
(self.z_limits_workspace[0] is not None and self.z_limits_workspace[1] is not None and self.z_limits_workspace[0] >= self.z_limits_workspace[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_workspace}")
print(f"y: {self.y_limits_workspace}")
print(f"z: {self.z_limits_workspace}")
con = True
while con:
confirm = input("Do you want your robot to move in this range? (y/n): ").strip().lower()
if confirm == 'y':
break
elif confirm == 'n':
print("Please re-enter the limits.")
con = False
else:
print("Invalid input. Please enter 'y' or 'n'.")
if con: break
except ValueError:
print("Invalid input. Please enter numeric values only.")
break
elif set_limits == 'n':
self.x_limits_workspace = [None, None]
self.y_limits_workspace = [None, None]
self.z_limits_workspace = [None, None]
break
print("Invalid input. Please enter 'y' or 'n'.")
# Ask the user if they want to set new joint limits
# Ask the user if they want to set new joint limits
while True:
print('+-'*50)
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)):
@@ -183,8 +231,8 @@ class OSC_ROS2_interface(Node):
while True:
try:
print("-" * 50)
lower_limit = input(f"Enter the new lower limit for joint '{joint}' (or press Enter to keep current): ").strip()
upper_limit = input(f"Enter the new upper limit for joint '{joint}' (or press Enter to keep current): ").strip()
lower_limit = input(f"Enter the new lower limit for joint '{joint}' (or press Enter for None): ").strip()
upper_limit = input(f"Enter the new upper limit for joint '{joint}' (or press Enter for None): ").strip()
if lower_limit and upper_limit and float(lower_limit) >= float(upper_limit):
print('--' * 50)
@@ -195,13 +243,13 @@ class OSC_ROS2_interface(Node):
break
except ValueError:
print("Invalid input. Please enter numeric values or leave blank to keep current limits.")
print(f'New limits for joint:\n lower: {self.joint_lim[0]}\n upper: {self.joint_lim[1]}')
break
elif update_limits == 'n':
self.joint_lim = None
break
print("Invalid input. Please enter 'y' or 'n'.")
print(f'New limits for joint:\n lower: {self.joint_lim[0]}\n upper: {self.joint_lim[1]}')
while True:
@@ -478,17 +526,20 @@ class OSC_ROS2_interface(Node):
f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[1][i]}."
)
else:
for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]):
if self.joint_lim[0][i] is not None and position < self.joint_lim[0][i]:
desired_joint_positions[i] = self.joint_lim[0][i]
self.get_logger().warn(
f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.joint_lim[0][i]}."
)
elif self.joint_lim[1][i] is not None and position > self.joint_lim[1][i]:
desired_joint_positions[i] = self.joint_lim[1][i]
self.get_logger().warn(
f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.joint_lim[1][i]}."
)
if self.joint_lim:
for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]):
if self.joint_lim[0][i] is not None:
if position < self.joint_lim[0][i]:
desired_joint_positions[i] = self.joint_lim[0][i]
self.get_logger().warn(
f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.joint_lim[0][i]}."
)
elif self.joint_lim[1][i] is not None:
if position > self.joint_lim[1][i]:
desired_joint_positions[i] = self.joint_lim[1][i]
self.get_logger().warn(
f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.joint_lim[1][i]}."
)
self.desired = ["joint_positions"] + desired_joint_positions
self.new = True
@@ -704,7 +755,7 @@ class OSC_ROS2_interface(Node):
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)
out_of_bounds = (fowards.t[1:,0] > self.x_limits_workspace[1] if self.x_limits_workspace[1] != None else False) | (fowards.t[1:,0] < self.x_limits_workspace[0] if self.x_limits_workspace[0] != None else False) | (fowards.t[1:,1] > self.y_limits_workspace[1] if self.y_limits_workspace[1] != None else False) | (fowards.t[1:,1] < self.y_limits_workspace[0] if self.y_limits_workspace[0] != None else False) | (fowards.t[1:,2] > self.z_limits_workspace[1] if self.z_limits_workspace[1] != None else False) | (fowards.t[1:,2] < self.z_limits_workspace[0] if self.z_limits_workspace[0] != None else False)
if np.any(out_of_bounds):
#print(fowards.t)
#indices = np.where(out_of_bounds)[0]
@@ -756,7 +807,7 @@ class OSC_ROS2_interface(Node):
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)
out_of_bounds = (fowards.t[1:,0] > self.x_limits_workspace[1] if self.x_limits_workspace[1] != None else False) | (fowards.t[1:,0] < self.x_limits_workspace[0] if self.x_limits_workspace[0] != None else False) | (fowards.t[1:,1] > self.y_limits_workspace[1] if self.y_limits_workspace[1] != None else False) | (fowards.t[1:,1] < self.y_limits_workspace[0] if self.y_limits_workspace[0] != None else False) | (fowards.t[1:,2] > self.z_limits_workspace[1] if self.z_limits_workspace[1] != None else False) | (fowards.t[1:,2] < self.z_limits_workspace[0] if self.z_limits_workspace[0] != None else False)
if np.any(out_of_bounds):
#print(fowards.t)
#indices = np.where(out_of_bounds)[0]