From 30a97aa19ae51a5878889d006a5991448c8cb3c4 Mon Sep 17 00:00:00 2001 From: Mohamed Fazil Date: Mon, 19 Aug 2024 16:02:11 -0700 Subject: [PATCH 1/3] Joint state msgs streaming position first pass --- stretch_core/stretch_core/stretch_driver.py | 84 ++++++++++++--------- 1 file changed, 50 insertions(+), 34 deletions(-) diff --git a/stretch_core/stretch_core/stretch_driver.py b/stretch_core/stretch_core/stretch_driver.py index d7daebff..974eb6fc 100755 --- a/stretch_core/stretch_core/stretch_driver.py +++ b/stretch_core/stretch_core/stretch_driver.py @@ -29,10 +29,8 @@ from nav_msgs.msg import Odometry from rcl_interfaces.msg import ParameterDescriptor, ParameterType, SetParametersResult from sensor_msgs.msg import BatteryState, JointState, Imu, MagneticField, Joy -from std_msgs.msg import Bool, String, Float64MultiArray - +from std_msgs.msg import Bool, String from hello_helpers.gripper_conversion import GripperConversion -from hello_helpers.joint_qpos_conversion import get_Idx, UnsupportedToolError from hello_helpers.hello_misc import LoopTimer from hello_helpers.gamepad_conversion import unpack_joy_to_gamepad_state, unpack_gamepad_state_to_joy, get_default_joy_msg from .joint_trajectory_server import JointTrajectoryAction @@ -113,7 +111,7 @@ def set_mobile_base_velocity_callback(self, twist): self.last_twist_time = self.get_clock().now() self.robot_mode_rwlock.release_read() - def set_robot_streaming_position_callback(self, msg): + def set_robot_streaming_position_callback(self, msg: JointState): self.robot_mode_rwlock.acquire_read() if not self.streaming_position_activated: self.get_logger().error('Streaming position is not activated.' @@ -132,40 +130,58 @@ def set_robot_streaming_position_callback(self, msg): if (self.get_clock().now().nanoseconds * 1e-9) - self.streaming_controller_lt.last_update_time > 5.0: print('Reset Streaming position looptimer after 5s no message received.') self.streaming_controller_lt.reset() - qpos = msg.data - self.move_to_position(qpos) + self.move_to_position(msg) self.robot_mode_rwlock.release_read() if STREAMING_POSITION_DEBUG: self.streaming_controller_lt.update() - def move_to_position(self, qpos): - try: - try: - Idx = get_Idx(self.robot.params['tool']) - except UnsupportedToolError: - self.get_logger().error('Unsupported tool for streaming position control.') - if len(qpos) != Idx.num_joints: - self.get_logger().error('Received qpos does not match the number of joints in the robot') - return - self.robot.arm.move_to(qpos[Idx.ARM]) - self.robot.lift.move_to(qpos[Idx.LIFT]) - self.robot.end_of_arm.move_to('wrist_yaw', qpos[Idx.WRIST_YAW]) - if 'wrist_pitch' in self.robot.end_of_arm.joints: - self.robot.end_of_arm.move_to('wrist_pitch', qpos[Idx.WRIST_PITCH]) - if 'wrist_roll' in self.robot.end_of_arm.joints: - self.robot.end_of_arm.move_to('wrist_roll', qpos[Idx.WRIST_ROLL]) - self.robot.head.move_to('head_pan', qpos[Idx.HEAD_PAN]) - self.robot.head.move_to('head_tilt', qpos[Idx.HEAD_TILT]) - if abs(qpos[Idx.BASE_TRANSLATE]) > 0.0 and abs(qpos[Idx.BASE_ROTATE]) > 0.0 and self.robot_mode != 'position': + def move_to_position(self, msg: JointState): + try: + if 'joint_base_translate' in msg.name and 'joint_base_rotate' in msg.name: self.get_logger().error('Cannot move base in both translation and rotation at the same time in position mode') - elif abs(qpos[Idx.BASE_TRANSLATE]) > 0.0 and self.robot_mode == 'position': - self.robot.base.translate_by(qpos[Idx.BASE_TRANSLATE]) - elif abs(qpos[Idx.BASE_ROTATE]) > 0.0 and self.robot_mode == 'position': - self.robot.base.rotate_by(qpos[Idx.BASE_ROTATE]) - if 'stretch_gripper' in self.robot.end_of_arm.joints: - pos = self.gripper_conversion.finger_to_robotis(qpos[Idx.GRIPPER]) - self.robot.end_of_arm.move_to('stretch_gripper', pos) - self.get_logger().info(f"Moved to position qpos: {qpos}") + return + + pos_length_match = len(msg.name) == len(msg.position) + if not pos_length_match: + self.get_logger().error('Streaming Position Command: Length of joint names and position list does not match.') + return + vel_present = len(msg.velocity) > 0 and len(msg.name) == len(msg.velocity) + if not vel_present: + self.get_logger().error('Streaming Position Command: Length of joint names and velocity list does not match.') + return + for joint,i in zip(msg.name, range(len(msg.name))): + if joint == 'joint_lift': + v = msg.velocity[i] if vel_present else None + self.robot.lift.move_to(msg.position[i], v) + elif joint == 'joint_arm': + v = msg.velocity[i] if vel_present else None + self.robot.arm.move_to(msg.position[i], v) + elif joint == 'joint_wrist_yaw': + v = msg.velocity[i] if vel_present else None + self.robot.end_of_arm.move_to('wrist_yaw', msg.position[i], v) + elif joint == 'joint_wrist_pitch'and 'wrist_pitch' in self.robot.end_of_arm.joints: + v = msg.velocity[i] if vel_present else None + self.robot.end_of_arm.move_to('wrist_pitch', msg.position[i], v) + elif joint == 'joint_wrist_roll' and 'wrist_roll' in self.robot.end_of_arm.joints: + v = msg.velocity[i] if vel_present else None + self.robot.end_of_arm.move_to('wrist_roll', msg.position[i], v) + elif joint == 'joint_head_pan': + v = msg.velocity[i] if vel_present else None + self.robot.head.move_to('head_pan', msg.position[i], v) + elif joint == 'joint_head_tilt': + v = msg.velocity[i] if vel_present else None + self.robot.head.move_to('head_tilt', msg.position[i], v) + elif joint == 'joint_base_translate': + v = msg.velocity[i] if vel_present else None + self.robot.base.translate_by(msg.position[i], v) + elif joint == 'joint_base_rotate': + v = msg.velocity[i] if vel_present else None + self.robot.base.rotate_by(msg.position[i], v) + elif joint == 'joint_gripper' and 'stretch_gripper' in self.robot.end_of_arm.joints: + v = msg.velocity[i] if vel_present else None + self.robot.end_of_arm.move_to('stretch_gripper', self.gripper_conversion.finger_to_robotis(msg.position[i]), v) + else: + self.get_logger().error(f'Joint name {msg.name[i]} not recognized.') except Exception as e: self.get_logger().error('Failed to move to position: {0}'.format(e)) @@ -989,7 +1005,7 @@ def ros_setup(self): self.create_subscription(Joy, "gamepad_joy", self.set_gamepad_motion_callback, 1, callback_group=self.group) - self.create_subscription(Float64MultiArray, "joint_pose_cmd", self.set_robot_streaming_position_callback, 1, callback_group=self.group) + self.create_subscription(JointState, "joint_pose_cmd", self.set_robot_streaming_position_callback, 1, callback_group=self.group) self.declare_parameter('rate', 30.0) self.joint_state_rate = self.get_parameter('rate').value From 6b0152b7ef96cbbf895e2a2a7be8aebc23a68783 Mon Sep 17 00:00:00 2001 From: Mohamed Fazil Date: Mon, 19 Aug 2024 17:34:50 -0700 Subject: [PATCH 2/3] Add perf tests --- stretch_core/stretch_core/stretch_driver.py | 62 +++--- ...streaming_joint_state_position_commands.py | 183 ++++++++++++++++++ 2 files changed, 208 insertions(+), 37 deletions(-) create mode 100644 stretch_core/test/test_streaming_joint_state_position_commands.py diff --git a/stretch_core/stretch_core/stretch_driver.py b/stretch_core/stretch_core/stretch_driver.py index 974eb6fc..03ed4691 100755 --- a/stretch_core/stretch_core/stretch_driver.py +++ b/stretch_core/stretch_core/stretch_driver.py @@ -35,10 +35,10 @@ from hello_helpers.gamepad_conversion import unpack_joy_to_gamepad_state, unpack_gamepad_state_to_joy, get_default_joy_msg from .joint_trajectory_server import JointTrajectoryAction from .stretch_diagnostics import StretchDiagnostics - +import time GRIPPER_DEBUG = False BACKLASH_DEBUG = False -STREAMING_POSITION_DEBUG = False +STREAMING_POSITION_DEBUG = True class StretchDriver(Node): @@ -145,43 +145,31 @@ def move_to_position(self, msg: JointState): if not pos_length_match: self.get_logger().error('Streaming Position Command: Length of joint names and position list does not match.') return - vel_present = len(msg.velocity) > 0 and len(msg.name) == len(msg.velocity) - if not vel_present: + + vel_present = len(msg.velocity) > 0 + if vel_present and len(msg.name) != len(msg.velocity): self.get_logger().error('Streaming Position Command: Length of joint names and velocity list does not match.') return - for joint,i in zip(msg.name, range(len(msg.name))): - if joint == 'joint_lift': - v = msg.velocity[i] if vel_present else None - self.robot.lift.move_to(msg.position[i], v) - elif joint == 'joint_arm': - v = msg.velocity[i] if vel_present else None - self.robot.arm.move_to(msg.position[i], v) - elif joint == 'joint_wrist_yaw': - v = msg.velocity[i] if vel_present else None - self.robot.end_of_arm.move_to('wrist_yaw', msg.position[i], v) - elif joint == 'joint_wrist_pitch'and 'wrist_pitch' in self.robot.end_of_arm.joints: - v = msg.velocity[i] if vel_present else None - self.robot.end_of_arm.move_to('wrist_pitch', msg.position[i], v) - elif joint == 'joint_wrist_roll' and 'wrist_roll' in self.robot.end_of_arm.joints: - v = msg.velocity[i] if vel_present else None - self.robot.end_of_arm.move_to('wrist_roll', msg.position[i], v) - elif joint == 'joint_head_pan': - v = msg.velocity[i] if vel_present else None - self.robot.head.move_to('head_pan', msg.position[i], v) - elif joint == 'joint_head_tilt': - v = msg.velocity[i] if vel_present else None - self.robot.head.move_to('head_tilt', msg.position[i], v) - elif joint == 'joint_base_translate': - v = msg.velocity[i] if vel_present else None - self.robot.base.translate_by(msg.position[i], v) - elif joint == 'joint_base_rotate': - v = msg.velocity[i] if vel_present else None - self.robot.base.rotate_by(msg.position[i], v) - elif joint == 'joint_gripper' and 'stretch_gripper' in self.robot.end_of_arm.joints: - v = msg.velocity[i] if vel_present else None - self.robot.end_of_arm.move_to('stretch_gripper', self.gripper_conversion.finger_to_robotis(msg.position[i]), v) - else: - self.get_logger().error(f'Joint name {msg.name[i]} not recognized.') + + joint_move_map = { + 'joint_lift': lambda pos, vel: self.robot.lift.move_to(pos, vel), + 'joint_arm': lambda pos, vel: self.robot.arm.move_to(pos, vel), + 'joint_wrist_yaw': lambda pos, vel: self.robot.end_of_arm.move_to('wrist_yaw', pos, vel), + 'joint_wrist_pitch': lambda pos, vel: self.robot.end_of_arm.move_to('wrist_pitch', pos, vel) if 'wrist_pitch' in self.robot.end_of_arm.joints else None, + 'joint_wrist_roll': lambda pos, vel: self.robot.end_of_arm.move_to('wrist_roll', pos, vel) if 'wrist_roll' in self.robot.end_of_arm.joints else None, + 'joint_head_pan': lambda pos, vel: self.robot.head.move_to('head_pan', pos, vel), + 'joint_head_tilt': lambda pos, vel: self.robot.head.move_to('head_tilt', pos, vel), + 'joint_base_translate': lambda pos, vel: self.robot.base.translate_by(pos, vel), + 'joint_base_rotate': lambda pos, vel: self.robot.base.rotate_by(pos, vel), + 'joint_gripper': lambda pos, vel: self.robot.end_of_arm.move_to('stretch_gripper', self.gripper_conversion.finger_to_robotis(pos), vel) if 'stretch_gripper' in self.robot.end_of_arm.joints else None, + } + + start = time.perf_counter() + for joint, pos, i in zip(msg.name, msg.position, range(len(msg.name))): + v = msg.velocity[i] if vel_present else None + if joint in joint_move_map: + joint_move_map[joint](pos, v) + print(f'Execution time: {(time.perf_counter() - start)*1000}ms') except Exception as e: self.get_logger().error('Failed to move to position: {0}'.format(e)) diff --git a/stretch_core/test/test_streaming_joint_state_position_commands.py b/stretch_core/test/test_streaming_joint_state_position_commands.py new file mode 100644 index 00000000..f5a928f4 --- /dev/null +++ b/stretch_core/test/test_streaming_joint_state_position_commands.py @@ -0,0 +1,183 @@ +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float64MultiArray +from hello_helpers.hello_misc import LoopTimer +from sensor_msgs.msg import JointState +from std_srvs.srv import Trigger +from hello_helpers.gripper_conversion import GripperConversion +from rclpy.callback_groups import ReentrantCallbackGroup +import numpy as np +import time + + + +class JointPosePublisher(Node): + def __init__(self): + rclpy.init() + super().__init__('float_array_publisher') + self.reentrant_cb = ReentrantCallbackGroup() + self.publisher_ = self.create_publisher(JointState, 'joint_pose_cmd', 10, callback_group=self.reentrant_cb) + # subscribe to joint states + self.joint_state = JointState() + self.joint_states_subscriber = self.create_subscription(JointState, + '/stretch/joint_states', + self.joint_states_callback, 10,callback_group=self.reentrant_cb) + self.switch_to_position_mode_service = self.create_client(Trigger, '/switch_to_position_mode',callback_group=self.reentrant_cb) + + self.switch_to_navigation_mode_service = self.create_client(Trigger, '/switch_to_navigation_mode',callback_group=self.reentrant_cb) + + self.activate_streaming_position_service = self.create_client(Trigger, '/activate_streaming_position', callback_group=self.reentrant_cb) + self.deactivate_streaming_position_service = self.create_client(Trigger, '/deactivate_streaming_position', callback_group=self.reentrant_cb) + + while not self.switch_to_position_mode_service.wait_for_service(timeout_sec=2.0): + self.get_logger().info("Waiting on '/switch_to_position_mode' service...") + self.gripper_conversion = GripperConversion() + self.lt = LoopTimer(print_debug=True) + + def joint_states_callback(self, msg): + self.joint_state = msg + + def switch_to_position_mode(self): + trigger_request = Trigger.Request() + trigger_result = self.switch_to_position_mode_service.call_async(trigger_request) + return + + def activate_streaming_position(self): + trigger_request = Trigger.Request() + trigger_result = self.activate_streaming_position_service.call_async(trigger_request) + time.sleep(1) + + def deactivate_streaming_position(self): + trigger_request = Trigger.Request() + trigger_result = self.deactivate_streaming_position_service.call_async(trigger_request) + return + + + def switch_to_navigation_mode(self): + trigger_request = Trigger.Request() + trigger_result = self.switch_to_navigation_mode_service.call_async(trigger_request) + return + + def parse_joint_state(self, joint_state_msg): + joint_status = {} + for name, position in zip(joint_state_msg.name, joint_state_msg.position): + joint_status[name] = position + return joint_status + + def publish_joint_pose(self, joint_pose): + self.publisher_.publish(joint_pose) + self.get_logger().info('Publishing: "%s"' % joint_pose) + +if __name__ == '__main__': + joint_pose_publisher = JointPosePublisher() + rclpy.spin_once(joint_pose_publisher) + + # joint_pose_publisher.switch_to_navigation_mode() + joint_pose_publisher.activate_streaming_position() + + home_pose = { + 'joint_lift': 0.6, + 'joint_arm': 0.0, + 'joint_wrist_yaw': 0.0, + 'joint_wrist_pitch': 0.0, + 'joint_wrist_roll': 0.0, + 'joint_head_pan': 0.0, + 'joint_head_tilt': 0.0, + # 'joint_base_translate', + # 'joint_base_rotate', + 'joint_gripper': 0 + } + goal = JointState() + for name, pos in home_pose.items(): + goal.name.append(name) + goal.position.append(pos) + joint_pose_publisher.publish_joint_pose(goal) + time.sleep(3) + + start = time.time() + target_rate = 30 + + i = 0 + joint_pose_publisher.lt.reset() + while i<100: + start = time.time() + i = 1 + i + rclpy.spin_once(joint_pose_publisher) + j_state = joint_pose_publisher.joint_state + goal = JointState() + for name, pos in zip(j_state.name, j_state.position): + if name == 'joint_lift': + goal.name.append('joint_lift') + goal.position.append(pos + 0.05) + elif name == "wrist_extension": + goal.name.append('joint_arm') + goal.position.append(pos + 0.05) + elif name == 'joint_wrist_pitch': + goal.name.append('joint_wrist_pitch') + goal.position.append(pos + 0.1) + elif name == 'joint_wrist_roll': + goal.name.append('joint_wrist_roll') + goal.position.append(pos + 0.1) + elif name == 'joint_wrist_yaw': + goal.name.append('joint_wrist_yaw') + goal.position.append(pos + 0.1) + elif name == 'joint_head_pan': + goal.name.append('joint_head_pan') + goal.position.append(pos + 0.1) + elif name == 'joint_head_tilt': + goal.name.append('joint_head_tilt') + goal.position.append(pos + 0.1) + elif name == 'joint_gripper_finger_left': + goal.name.append('joint_gripper') + goal.position.append(pos + 0.1) + joint_pose_publisher.publish_joint_pose(goal) + joint_pose_publisher.lt.update() + elp = time.time()-start + if elp<1/target_rate: + time.sleep((1/target_rate)-elp) + + + i = 0 + joint_pose_publisher.lt.reset() + while i<100: + start = time.time() + i = 1 + i + rclpy.spin_once(joint_pose_publisher) + j_state = joint_pose_publisher.joint_state + print(j_state) + goal = JointState() + for name, pos in zip(j_state.name, j_state.position): + if name == 'joint_lift': + goal.name.append('joint_lift') + goal.position.append(pos - 0.05) + elif name == "wrist_extension": + goal.name.append('joint_arm') + goal.position.append(pos - 0.05) + elif name == 'joint_wrist_pitch': + goal.name.append('joint_wrist_pitch') + goal.position.append(pos - 0.1) + elif name == 'joint_wrist_roll': + goal.name.append('joint_wrist_roll') + goal.position.append(pos - 0.1) + elif name == 'joint_wrist_yaw': + goal.name.append('joint_wrist_yaw') + goal.position.append(pos - 0.1) + elif name == 'joint_head_pan': + goal.name.append('joint_head_pan') + goal.position.append(pos - 0.1) + elif name == 'joint_head_tilt': + goal.name.append('joint_head_tilt') + goal.position.append(pos - 0.1) + elif name == 'joint_gripper_finger_left': + goal.name.append('joint_gripper') + goal.position.append(pos - 0.1) + joint_pose_publisher.publish_joint_pose(goal) + joint_pose_publisher.lt.update() + elp = time.time()-start + if elp<1/target_rate: + time.sleep((1/target_rate)-elp) + + + + joint_pose_publisher.destroy_node() + rclpy.shutdown() \ No newline at end of file From 70c0c0d21778fcd5e17f971772c128d92228db90 Mon Sep 17 00:00:00 2001 From: Mohamed Fazil Date: Mon, 19 Aug 2024 17:43:00 -0700 Subject: [PATCH 3/3] Remove unused modules --- stretch_core/stretch_core/stretch_driver.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/stretch_core/stretch_core/stretch_driver.py b/stretch_core/stretch_core/stretch_driver.py index 03ed4691..078b710a 100755 --- a/stretch_core/stretch_core/stretch_driver.py +++ b/stretch_core/stretch_core/stretch_driver.py @@ -35,7 +35,7 @@ from hello_helpers.gamepad_conversion import unpack_joy_to_gamepad_state, unpack_gamepad_state_to_joy, get_default_joy_msg from .joint_trajectory_server import JointTrajectoryAction from .stretch_diagnostics import StretchDiagnostics -import time + GRIPPER_DEBUG = False BACKLASH_DEBUG = False STREAMING_POSITION_DEBUG = True @@ -164,12 +164,10 @@ def move_to_position(self, msg: JointState): 'joint_gripper': lambda pos, vel: self.robot.end_of_arm.move_to('stretch_gripper', self.gripper_conversion.finger_to_robotis(pos), vel) if 'stretch_gripper' in self.robot.end_of_arm.joints else None, } - start = time.perf_counter() for joint, pos, i in zip(msg.name, msg.position, range(len(msg.name))): v = msg.velocity[i] if vel_present else None if joint in joint_move_map: joint_move_map[joint](pos, v) - print(f'Execution time: {(time.perf_counter() - start)*1000}ms') except Exception as e: self.get_logger().error('Failed to move to position: {0}'.format(e))