What happened?
The following behaviours are possible:
- The robot could wrongly reject commands due to too conservative velocity limit checking
- The robot could ignore the configured velocity limits, potentially creating unsafe behaviour that can damage the robot and/or its environment.
This happens when different movetimes are used throughout a single script.
The reason is a bug in _check_joint_limits():
|
def _check_joint_limits(self, positions: List[float]) -> bool: |
|
""" |
|
Ensure the desired arm group's joint positions are within their limits. |
|
|
|
:param positions: the positions [rad] to check |
|
:return: `True` if all positions are within limits; `False` otherwise |
|
""" |
|
self.core.get_node().logdebug(f'Checking joint limits for {positions=}') |
|
|
|
# Reject any commands containing NaN values |
|
if any(math.isnan(elem) for elem in positions): |
|
self.core.get_node().logwarn('Rejecting NaN values in position command') |
|
return False |
|
|
|
theta_list = [int(elem * 1000) / 1000.0 for elem in positions] |
|
speed_list = [ |
|
abs(goal - current) / float(self.moving_time) |
|
for goal, current in zip(theta_list, self.joint_commands) |
|
] |
|
# check position and velocity limits |
|
for x in range(self.group_info.num_joints): |
|
if not ( |
|
self.group_info.joint_lower_limits[x] |
|
<= theta_list[x] |
|
<= self.group_info.joint_upper_limits[x] |
|
): |
|
return False |
|
if speed_list[x] > self.group_info.joint_velocity_limits[x]: |
|
return False |
|
return True |
The velocities are being calculated using self.moving_time. However, during the evaluation of these velocities, the moving self.moving_time is still set to the moving time of the last command, NOT the command that we are trying to send now! For example, in set_joint_positions(), we can see that the supplied moving time is not set before evaluating joint limits, nor used for evaluating joint limits, thus wrongly evaluating if limits are adhered to:
|
def set_joint_positions( |
|
self, |
|
joint_positions: List[float], |
|
moving_time: float = None, |
|
accel_time: float = None, |
|
blocking: bool = True, |
|
) -> bool: |
|
""" |
|
Command positions to the arm joints. |
|
|
|
:param joint_positions: desired joint positions [rad] |
|
:param moving_time: (optional) duration in seconds that the robot should move |
|
:param accel_time: (optional) duration in seconds that that robot should spend |
|
accelerating/decelerating (must be less than or equal to half the moving_time) |
|
:param blocking: (optional) whether the function should wait to return control to the user |
|
until the robot finishes moving |
|
:return: `True` if position was commanded; `False` if it wasn't due to being outside limits |
|
""" |
|
self.core.get_node().logdebug(f'Setting {joint_positions=}') |
|
if self._check_joint_limits(joint_positions): |
|
self._publish_commands(joint_positions, moving_time, accel_time, blocking) |
|
return True |
|
else: |
|
return False |
I can imagine this also happens for other command interfaces.
Robot Model
xs robots
Operating System
Ubuntu 22.04
ROS Distro
ROS 2 Rolling, ROS 2 Humble
Steps To Reproduce
First, send a command with a low moving time and a short distance to travel. Then, send a command with a high moving time and a bigger distance to travel. The second command will be rejected even though it adheres to the velocity limits. (This is how we found the issue)
[Unsafe behaviour!] Alternatively, first sending a command with a high moving time, and consequently a command with a short moving time but long distance to travel, the command might still get executed even though it violates the configured velocity limits. (We did not test this because we did not want to play with fire)
Relevant log output
Additional Info
No response
