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

AS: redame updated

Browse Source
This commit is contained in:
Alexander Schaefer
2025-05-15 14:11:55 +02:00
parent ff8d5ee281
commit 161bb068df
2 changed files with 95 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -349,20 +349,33 @@ class OSC_ROS2_interface(Node):
"""Handles incoming OSC messages for joint positions."""
try:
joint_name = address.split("/")[-1]
if joint_name in self.joint_names:
if len(args) == 1:
position = float(args[0])
index = self.joint_names.index(joint_name)
if position < self.robot.qlim[0][index]:
position = self.robot.qlim[0][index]
self.get_logger().warn(
f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[0][index]}."
)
elif position > self.robot.qlim[1][index]:
position = self.robot.qlim[1][index]
self.get_logger().warn(
f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[1][index]}."
)
if self.robot:
if position < self.robot.qlim[0][index]:
position = self.robot.qlim[0][index]
self.get_logger().warn(
f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[0][index]}."
)
elif position > self.robot.qlim[1][index]:
position = self.robot.qlim[1][index]
self.get_logger().warn(
f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[1][index]}."
)
else:
if self.joint_lim[0][index] is not None and position < self.joint_lim[0][index]:
position = self.joint_lim[0][index]
self.get_logger().warn(
f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[0][index]}."
)
elif self.joint_lim[1][index] is not None and position > self.joint_lim[1][index]:
position = self.joint_lim[1][index]
self.get_logger().warn(
f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[1][index]}."
)
desired_joint_positions = self.current_joint_positions
desired_joint_positions[index] = position
self.desired = ["joint_positions"] + desired_joint_positions
@@ -371,16 +384,28 @@ class OSC_ROS2_interface(Node):
position = float(args[0])
duration = float(args[1])
index = self.joint_names.index(joint_name)
if position < self.robot.qlim[0][index]:
position = self.robot.qlim[0][index]
self.get_logger().warn(
f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[0][index]}."
)
elif position > self.robot.qlim[1][index]:
position = self.robot.qlim[1][index]
self.get_logger().warn(
f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[1][index]}."
)
if self.robot:
if position < self.robot.qlim[0][index]:
position = self.robot.qlim[0][index]
self.get_logger().warn(
f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[0][index]}."
)
elif position > self.robot.qlim[1][index]:
position = self.robot.qlim[1][index]
self.get_logger().warn(
f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[1][index]}."
)
else:
if self.joint_lim[0][index] is not None and position < self.joint_lim[0][index]:
position = self.joint_lim[0][index]
self.get_logger().warn(
f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[0][index]}."
)
elif self.joint_lim[1][index] is not None and position > self.joint_lim[1][index]:
position = self.joint_lim[1][index]
self.get_logger().warn(
f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[1][index]}."
)
desired_joint_positions = self.current_joint_positions
desired_joint_positions[index] = position
self.desired = ["joint_positions"] + desired_joint_positions + [duration]
@@ -413,17 +438,30 @@ class OSC_ROS2_interface(Node):
return
# Check if joint positions exceed limits
for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]): # Exclude duration if present
if position < self.robot.qlim[0][i]:
desired_joint_positions[i] = self.robot.qlim[0][i]
self.get_logger().warn(
f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[0][i]}."
)
elif position > self.robot.qlim[1][i]:
desired_joint_positions[i] = self.robot.qlim[1][i]
self.get_logger().warn(
f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[1][i]}."
)
if self.robot:
for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]): # Exclude duration if present
if position < self.robot.qlim[0][i]:
desired_joint_positions[i] = self.robot.qlim[0][i]
self.get_logger().warn(
f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[0][i]}."
)
elif position > self.robot.qlim[1][i]:
desired_joint_positions[i] = self.robot.qlim[1][i]
self.get_logger().warn(
f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[1][i]}."
)
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]}."
)
self.desired = ["joint_positions"] + desired_joint_positions
self.new = True