From 9440843ded85f22989ee985c5919cb1ace367865 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Wed, 19 Jan 2022 16:06:18 -0800 Subject: [PATCH] Include dex wrist joints in keyboard_teleop --- stretch_core/nodes/keyboard_teleop | 70 +++++++++++++++++++++--------- 1 file changed, 49 insertions(+), 21 deletions(-) diff --git a/stretch_core/nodes/keyboard_teleop b/stretch_core/nodes/keyboard_teleop index e7f2bc8..1a6131c 100755 --- a/stretch_core/nodes/keyboard_teleop +++ b/stretch_core/nodes/keyboard_teleop @@ -50,33 +50,56 @@ class GetKeyboardCommands: deltas = {'rad': self.big_rad, 'translate': self.big_translate} return deltas - def print_commands(self): + def print_commands(self, joint_state, command): + if command is None: + return + + joints = joint_state.name + def in_joints(i): + return len(list(set(i) & set(joints))) > 0 + print('---------- KEYBOARD TELEOP MENU -----------') print(' ') - print(' i HEAD UP ') - print(' j HEAD LEFT l HEAD RIGHT ') - print(' , HEAD DOWN ') + if in_joints(['joint_head_tilt']): + print(' i HEAD UP ') + if in_joints(['joint_head_pan']): + print(' j HEAD LEFT l HEAD RIGHT ') + if in_joints(['joint_head_tilt']): + print(' , HEAD DOWN ') print(' ') print(' ') print(' 7 BASE ROTATE LEFT 9 BASE ROTATE RIGHT') print(' home page-up ') print(' ') print(' ') - print(' 8 LIFT UP ') - print(' up-arrow ') + if in_joints(['joint_lift']): + print(' 8 LIFT UP ') + print(' up-arrow ') print(' 4 BASE FORWARD 6 BASE BACK ') print(' left-arrow right-arrow ') - print(' 2 LIFT DOWN ') - print(' down-arrow ') + if in_joints(['joint_lift']): + print(' 2 LIFT DOWN ') + print(' down-arrow ') print(' ') print(' ') - print(' w ARM OUT ') - print(' a WRIST FORWARD d WRIST BACK ') - print(' x ARM IN ') + if in_joints(['joint_arm_l0', 'joint_arm_l1', 'joint_arm_l2', 'joint_arm_l3']): + print(' w ARM OUT ') + if in_joints(['joint_wrist_yaw']): + print(' a WRIST FORWARD d WRIST BACK ') + if in_joints(['joint_arm_l0', 'joint_arm_l1', 'joint_arm_l2', 'joint_arm_l3']): + print(' x ARM IN ') + if in_joints(['joint_wrist_pitch', 'joint_wrist_roll']): + print(' ') + print(' ') + if in_joints(['joint_wrist_pitch']): + print(' c PITCH FORWARD v PITCH BACK ') + if in_joints(['joint_wrist_roll']): + print(' o ROLL FORWARD p ROLL BACK ') print(' ') print(' ') - print(' 5 GRIPPER CLOSE ') - print(' 0 GRIPPER OPEN ') + if in_joints(['joint_gripper_finger_left', 'joint_gripper_finger_right', 'gripper_aperture']): + print(' 5 GRIPPER CLOSE ') + print(' 0 GRIPPER OPEN ') print(' ') print(' step size: b BIG, m MEDIUM, s SMALL ') print(' ') @@ -234,6 +257,14 @@ class GetKeyboardCommands: command = {'joint': 'joint_wrist_yaw', 'delta': -self.get_deltas()['rad']} if c == 'a' or c == 'A': command = {'joint': 'joint_wrist_yaw', 'delta': self.get_deltas()['rad']} + if c == 'v' or c == 'V': + command = {'joint': 'joint_wrist_pitch', 'delta': -self.get_deltas()['rad']} + if c == 'c' or c == 'C': + command = {'joint': 'joint_wrist_pitch', 'delta': self.get_deltas()['rad']} + if c == 'p' or c == 'P': + command = {'joint': 'joint_wrist_roll', 'delta': -self.get_deltas()['rad']} + if c == 'o' or c == 'O': + command = {'joint': 'joint_wrist_roll', 'delta': self.get_deltas()['rad']} if c == '5' or c == '\x1b[E' or c == 'g' or c == 'G': # grasp command = {'joint': 'joint_gripper_finger_left', 'delta': -self.get_deltas()['rad']} @@ -295,20 +326,16 @@ class KeyboardTeleopNode(hm.HelloNode): trajectory_goal.trajectory.joint_names = [joint_name] if 'inc' in command: inc = command['inc'] - rospy.loginfo('inc = {0}'.format(inc)) new_value = inc elif 'delta' in command: joint_index = joint_state.name.index(joint_name) joint_value = joint_state.position[joint_index] delta = command['delta'] - rospy.loginfo('delta = {0}, joint_index = {1}, joint_value = {2}'.format(delta, joint_index, joint_value)) new_value = joint_value + delta point.positions = [new_value] trajectory_goal.trajectory.points = [point] trajectory_goal.trajectory.header.stamp = rospy.Time.now() - rospy.loginfo('joint_name = {0}, trajectory_goal = {1}'.format(joint_name, trajectory_goal)) self.trajectory_client.send_goal(trajectory_goal) - rospy.loginfo('Done sending pose.') def main(self): hm.HelloNode.main(self, 'keyboard_teleop', 'keyboard_teleop', wait_for_first_pointcloud=False) @@ -378,11 +405,12 @@ class KeyboardTeleopNode(hm.HelloNode): rate = rospy.Rate(self.rate) + command = 1 # set equal to not None, so menu is printed out on first loop while not rospy.is_shutdown(): - self.keys.print_commands() - command = self.keys.get_command(self) - rospy.loginfo(command) - self.send_command(command) + if self.joint_state is not None: + self.keys.print_commands(self.joint_state, command) + command = self.keys.get_command(self) + self.send_command(command) rate.sleep()