Skip to content

Consider commanded moving_time in joint limit check #90

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: humble
Choose a base branch
from
Open

Consider commanded moving_time in joint limit check #90

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3f5dfe7
Fix joint limit timing check
lukeschmitt-tr Aug 7, 2025
43977d9
Fix single joint limit check
lukeschmitt-tr Aug 7, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
31 changes: 24 additions & 7 deletions interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/arm.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@

import math
import sys
from typing import Any, List, Tuple, Union
from typing import Any, List, Optional, Tuple, Union

import interbotix_common_modules.angle_manipulation as ang
from interbotix_common_modules.common_robot.robot import InterbotixRobotNode
Expand Down Expand Up @@ -318,23 +318,31 @@ def set_trajectory_time(
)
self.core.get_node().wait_until_future_complete(future_accel_time)

def _check_joint_limits(self, positions: List[float]) -> bool:
def _check_joint_limits(
self,
positions: List[float],
moving_time: Optional[float] = None,
) -> bool:
"""
Ensure the desired arm group's joint positions are within their limits.

:param positions: the positions [rad] to check
:param moving_time: (optional) duration in seconds that the robot should move
:return: `True` if all positions are within limits; `False` otherwise
"""
self.core.get_node().logdebug(f'Checking joint limits for {positions=}')

if moving_time is None:
moving_time = self.moving_time

# 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)
abs(goal - current) / float(moving_time)
for goal, current in zip(theta_list, self.joint_commands)
]
# check position and velocity limits
Expand All @@ -349,18 +357,27 @@ def _check_joint_limits(self, positions: List[float]) -> bool:
return False
return True

def _check_single_joint_limit(self, joint_name: str, position: float) -> bool:
def _check_single_joint_limit(
self,
joint_name: str,
position: float,
moving_time: Optional[float],
) -> bool:
"""
Ensure a desired position for a given joint is within its limits.

:param joint_name: desired joint name
:param position: desired joint position [rad]
:param moving_time: (optional) duration in seconds that the robot should move
:return: `True` if within limits; `False` otherwise
"""
self.core.get_node().logdebug(
f"Checking joint '{joint_name}' limits for {position=}"
)

if moving_time is None:
moving_time = self.moving_time

# Reject any commands containing NaN values
if math.isnan(position):
self.core.get_node().logwarn('Rejecting NaN value in position command')
Expand All @@ -369,7 +386,7 @@ def _check_single_joint_limit(self, joint_name: str, position: float) -> bool:
theta = int(position * 1000) / 1000.0
speed = abs(
theta - self.joint_commands[self.info_index_map[joint_name]]
) / float(self.moving_time)
) / float(moving_time)
ll = self.group_info.joint_lower_limits[self.info_index_map[joint_name]]
ul = self.group_info.joint_upper_limits[self.info_index_map[joint_name]]
vl = self.group_info.joint_velocity_limits[self.info_index_map[joint_name]]
Expand Down Expand Up @@ -398,7 +415,7 @@ def set_joint_positions(
: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):
if self._check_joint_limits(joint_positions, moving_time):
self._publish_commands(joint_positions, moving_time, accel_time, blocking)
return True
else:
Expand Down Expand Up @@ -534,7 +551,7 @@ def set_ee_pose_matrix(
# Check to make sure a solution was found and that no joint limits were violated
if success:
theta_list = self._wrap_theta_list(theta_list)
solution_found = self._check_joint_limits(theta_list)
solution_found = self._check_joint_limits(theta_list, moving_time)
else:
solution_found = False

Expand Down
Loading