def on_play_2(self): change_to_controller('position') pm = SimpleActionClient('/play_motion', PlayMotionAction) pm.wait_for_server() pmg = PlayMotionGoal() pmg.motion_name = 'LBD_2X' pm.send_goal(pmg) pm.wait_for_result(rospy.Duration(0.1))
def __init__(self, parent=None): super(LearningGUI, self).__init__(parent) rospy.loginfo("Getting joint limits from URDF...") self.joint_limits = get_joint_limits() self.rospack = rospkg.RosPack() self.continuous_recording_mode_active = False path = self.rospack.get_path('learning_gui') print "path is: " + str(path) # Decide which GUI to use thanks to joint states... # set subscriber self.last_js = None self.joint_states_sub = rospy.Subscriber('/joint_states', JointState, self.js_cb, queue_size=1) rospy.loginfo("Waiting for first joint states...") while self.last_js is None: rospy.sleep(0.2) rospy.loginfo("Guessing end effector in use...") if "hand_thumb_joint" in self.last_js.name: uic.loadUi(path + '/resources/tiago_learn_hand.ui', self) self.joint_names = [ 'head_1_joint', 'head_2_joint', 'arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint', 'arm_5_joint', 'arm_6_joint', 'arm_7_joint', 'torso_lift_joint', 'hand_index_joint', 'hand_mrl_joint', 'hand_thumb_joint' ] self.eef = 'hand' elif "gripper_left_finger_joint" in self.last_js.name: uic.loadUi(path + '/resources/tiago_learn_gripper.ui', self) self.joint_names = [ 'head_1_joint', 'head_2_joint', 'arm_1_joint', 'arm_2_joint', 'arm_3_joint', 'arm_4_joint', 'arm_5_joint', 'arm_6_joint', 'arm_7_joint', 'torso_lift_joint', 'gripper_left_finger_joint', 'gripper_right_finger_joint' ] self.eef = 'gripper' self.set_sliders_limits() self.set_callbacks_sliders() self.controllers_srv = rospy.ServiceProxy( '/controller_manager/list_controllers', ListControllers) change_to_controller('position') self.controller_mode = 'position' self.gui_disabled_status = { 'gravity_b': False, 'position_b': True, 'start_continuous': False, 'stop_recording': True, 'capture_waypoint': False, 'play_05_b': True, 'play_1_b': True, 'play_2_b': True, 'save_b': True, 'arm_1_slider': False, # For wrist controller 'arm_2_slider': False, 'arm_3_slider': False, 'arm_4_slider': False } self.set_buttons_disabled() # check if we are in gravity to enable stuff related to it like # commanding with the wrist self.set_callbacks_buttons() self.show() self.update_gui_rate = 2.0 # Hz self.arm_pub = rospy.Publisher('/arm_controller/command', JointTrajectory, queue_size=1) self.wrist_pub = rospy.Publisher('/wrist_controller/command', JointTrajectory, queue_size=1) self.head_pub = rospy.Publisher('/head_controller/command', JointTrajectory, queue_size=1) self.torso_pub = rospy.Publisher('/torso_controller/command', JointTrajectory, queue_size=1) self.hand_pub = rospy.Publisher('/hand_controller/command', JointTrajectory, queue_size=1) self.gripper_pub = rospy.Publisher('/gripper_controller/command', JointTrajectory, queue_size=1) # self.play_motion_pub = rospy.Publisher( # '/play_motion/goal', PlayMotionActionGoal, queue_size=1) self.update_timer = QTimer(self) self.update_timer.setInterval(1000.0 / self.update_gui_rate) self.update_timer.timeout.connect(self.update_gui) self.update_timer.start()
def on_position(self): change_to_controller('position') self.gui_disabled_status['position_b'] = True self.gui_disabled_status['gravity_b'] = False
def on_gravity(self): change_to_controller('gravity', self.use_wrist_in_position) self.gui_disabled_status['position_b'] = False self.gui_disabled_status['gravity_b'] = True