diff --git a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/arm.py b/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/arm.py index 14fbcfa..ff00448 100644 --- a/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/arm.py +++ b/interbotix_xs_toolbox/interbotix_xs_modules/interbotix_xs_modules/xs_robot/arm.py @@ -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 @@ -318,15 +318,23 @@ 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') @@ -334,7 +342,7 @@ def _check_joint_limits(self, positions: List[float]) -> bool: 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 @@ -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') @@ -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]] @@ -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: @@ -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