def main(): ################################################################################################################## rospy.init_node('ExampleHandNode') # Services can automatically call hand calibration calibrate_fingers = rospy.ServiceProxy('/reflex_plus/calibrate_fingers', Empty) # This collection of publishers can be used to command the hand command_pub = rospy.Publisher('/reflex_plus/command', Command, queue_size=1) pos_pub = rospy.Publisher('/reflex_plus/command_position', PoseCommand, queue_size=1) vel_pub = rospy.Publisher('/reflex_plus/command_velocity', VelocityCommand, queue_size=1) # force_pub = rospy.Publisher('/reflex_plus/command_motor_force', ForceCommand, queue_size=1) # Constantly capture the current hand state rospy.Subscriber('/reflex_plus/hand_state', Hand, hand_state_cb) raw_input( "== When ready to wiggle fingers with position control, hit [Enter]\n") for i in range(10000): # setpoint = (-cos(i / 5.0) + 1) * 1.75 pos_pub.publish(PoseCommand(f1=0, f2=0, f3=0, preshape=.5)) rospy.sleep(2) pos_pub.publish(PoseCommand(f1=0, f2=0, f3=0, preshape=0.0)) rospy.sleep(2) print(i)
def main(): ################################################################################################################## rospy.init_node('ExampleHandNode') # Services can automatically call hand calibration calibrate_fingers = rospy.ServiceProxy('/reflex_takktile/calibrate_fingers', Empty) calibrate_tactile = rospy.ServiceProxy('/reflex_takktile/calibrate_tactile', Empty) # Services can set tactile thresholds and enable tactile stops enable_tactile_stops = rospy.ServiceProxy('/reflex_takktile/enable_tactile_stops', Empty) disable_tactile_stops = rospy.ServiceProxy('/reflex_takktile/disable_tactile_stops', Empty) set_tactile_threshold = rospy.ServiceProxy('/reflex_takktile/set_tactile_threshold', SetTactileThreshold) # This collection of publishers can be used to command the hand command_pub = rospy.Publisher('/reflex_takktile/command', Command, queue_size=1) pos_pub = rospy.Publisher('/reflex_takktile/command_position', PoseCommand, queue_size=1) vel_pub = rospy.Publisher('/reflex_takktile/command_velocity', VelocityCommand, queue_size=1) force_pub = rospy.Publisher('/reflex_takktile/command_motor_force', ForceCommand, queue_size=1) # Constantly capture the current hand state rospy.Subscriber('/reflex_takktile/hand_state', Hand, hand_state_cb) ################################################################################################################## # Calibrate the fingers (make sure hand is opened in zero position) raw_input("== When ready to calibrate the hand, press [Enter]\n") calibrate_fingers() raw_input("...\n") ################################################################################################################## # Demonstration of position control raw_input("== When ready to wiggle fingers with position control, hit [Enter]\n") for i in range(10): pos_pub.publish(PoseCommand(f1=2.7, f2=2.7, f3=2.7, preshape=0.0)) rospy.sleep(1.5) pos_pub.publish(PoseCommand()) rospy.sleep(1) raw_input("...\n") ################################################################################################################## # Demonstration of preshape joint raw_input("== When ready to test preshape joint, hit [Enter]\n") for i in range (10): pos_pub.publish(PoseCommand(preshape=1.57)) rospy.sleep(1.0) pos_pub.publish(PoseCommand(f1=2.7, f2=2.7, f3=0, preshape=1.57)) rospy.sleep(2.0) pos_pub.publish(PoseCommand()) rospy.sleep(2.0) raw_input("...\n")
def handleButtonHome(self): """ Reset fingers to home positions """ pose = PoseCommand(f1=0.0, f2=0.0, f3=0.0, k1=0.0, k2=1.0) vel = VelocityCommand(f1=1, f2=1, f3=1, k1=1, k2=1) home_command = Command(pose=pose, velocity=vel) self.moveHandtoPose(home_command)
def stop_hand(var): speed = 0.05 * var vel_pub.publish(VelocityCommand()) pos_pub.publish( PoseCommand(hand_state.motor[0].joint_angle + speed, hand_state.motor[1].joint_angle + speed, hand_state.motor[2].joint_angle + speed, hand_state.motor[3].joint_angle))
def open_hand(): disable_tactile_stops = rospy.ServiceProxy( '/reflex_takktile/disable_tactile_stops', Empty) pos_pub = rospy.Publisher('/reflex_takktile/command_position', PoseCommand, queue_size=1) disable_tactile_stops() pos_pub.publish(PoseCommand(preshape=0.0))
def getDelay(self): """ Prompt for user input to set a delay """ num, ok = QInputDialog.getDouble(self, "Delay", "Delay in seconds") if ok: pose0 = PoseCommand(f1=999, f2=999, f3=999, k1=999, k2=num) vel0 = VelocityCommand(f1=999, f2=999, f3=999, k1=999, k2=num) return Command(pose=pose0, velocity=vel0) else: return False
def handleButtonGo(self): tar_f1 = float(self.finger_slider_1.value()) / 100.0 tar_f2 = float(self.finger_slider_2.value()) / 100.0 tar_f3 = float(self.finger_slider_3.value()) / 100.0 tar_f4 = float(self.finger_slider_4.value()) / 100.0 poseTarget = PoseCommand(f1=tar_f1, f2=tar_f2, f3=tar_f3, preshape=tar_f4) self.command_pub.publish(poseTarget) print "Go Button Click"
def readFile(self, file_name): """ Decode the file with the given file name return the stored list of commands """ pose_list = [] try: file_path = "{}/{}".format(FILE_DIR, file_name) file = open(file_path, 'r').read() # Divide file into poses data_chunks = file.split('//') data_chunks.pop(0) #the file gets saved with an extra // trigger for command in data_chunks: p_or_v = command.split('***') cnt = 0 for test in p_or_v: # divide each pose up by commands per finger f1, f2, f3, k1, k2 = test.split('\n') # choose only the numerical chunk of command and convert to float tar_f1 = float(f1.split(': ')[1]) tar_f2 = float(f2.split(': ')[1]) tar_f3 = float(f3.split(': ')[1]) tar_k1 = float(k1.split(': ')[1]) tar_k2 = float(k2.split(': ')[1]) if cnt == 0: pose0 = PoseCommand(f1=tar_f1, f2=tar_f2, f3=tar_f3, k1=tar_k1, k2=tar_k2) cnt = cnt + 1 else: velocity0 = VelocityCommand(f1=tar_f1, f2=tar_f2, f3=tar_f3, k1=tar_k1, k2=tar_k2) cmd = Command(pose=pose0, velocity=velocity0) pose_list.append(cmd) except AttributeError: error_msg = QErrorMessage(self) error_msg.setWindowTitle("File Error") error_msg.showMessage("Please Select A File to Execute") pose_list = False except Exception: error_msg2 = QErrorMessage(self) error_msg2.setWindowTitle("File Error") error_msg2.showMessage("Could not load file") pose_list = False return pose_list
def _tighten_hand(self, pos_increment=0.8): self._disable_tactile_stops() pos_cmd = PoseCommand() pos_cmd.f1 = self.hand_state.finger[0].proximal + pos_increment pos_cmd.f2 = self.hand_state.finger[1].proximal + pos_increment pos_cmd.f3 = self.hand_state.finger[2].proximal + pos_increment pos_cmd.preshape = 0.0 for i in range(10): self.pos_cmd_pub.publish(pos_cmd)
def getSliderPose(self): """ return the current pose of the slider bars """ float_value_1 = float(self.value_slider_1.text()) float_value_2 = float(self.value_slider_2.text()) float_value_3 = float(self.value_slider_3.text()) float_value_4 = float(self.value_slider_4.text()) float_value_5 = float(self.value_slider_5.text()) pose0 = PoseCommand(f1=float_value_1, f2=float_value_2, f3=float_value_3, k1=float_value_4, k2=1 + float_value_5) return pose0
def open_hand(self, req): pos_cmd = PoseCommand() pos_cmd.f1 = 0.0 pos_cmd.f2 = 0.0 pos_cmd.f3 = 0.0 pos_cmd.preshape = 0.0 for i in range(10): self.pos_cmd_pub.publish(pos_cmd) self.grasp_status = False return TriggerResponse(success=True)
def main(): ################################################################################################################## rospy.init_node('ExampleHandNode') # Services can automatically call hand calibration prefix = '/reflex_hand2' calibrate_fingers = rospy.ServiceProxy(prefix + '/calibrate_fingers', Empty) # This collection of publishers can be used to command the hand command_pub = rospy.Publisher(prefix + '/command', Command, queue_size=10) pos_pub = rospy.Publisher(prefix + '/command_position', PoseCommand, queue_size=10) vel_pub = rospy.Publisher(prefix + '/command_velocity', VelocityCommand, queue_size=10) # force_pub = rospy.Publisher('/reflex_sf/command_motor_force', ForceCommand, queue_size=1) # Constantly capture the current hand state rospy.Subscriber(prefix + '/hand_state', Hand, hand_state_cb) ################################################################################################################## # Calibrate the fingers (make sure hand is opened in zero position) #raw_input("== When ready to calibrate the hand, press [Enter]\n") #print('Go to the window where reflex_sf was run, you will see the calibration prompts') #calibrate_fingers() #raw_input("...\n") ################################################################################################################## # Demonstration of position control raw_input( "== When ready to wiggle fingers with position control, hit [Enter]\n") for i in range(4): setpoint = (i % 2) * 1.5 pos_pub.publish( PoseCommand(f1=setpoint, f2=setpoint, f3=setpoint, preshape=0.0)) rospy.sleep(0.1) raw_input("...\n")
def processData(self, values): """ Callback for glove data msg. convert message to Command() and add to list """ tar_f1 = float(values.data[0]) / 100 tar_f2 = float(values.data[1]) / 100 tar_f3 = float(values.data[2]) / 100 tar_k1 = float(values.data[3]) / 100 tar_k2 = (float(values.data[4]) / 100) - 1 pose = PoseCommand(f1=tar_f1, f2=tar_f2, f3=tar_f3, k1=tar_k1, k2=tar_k2) vel = VelocityCommand(f1=1.5, f2=1.5, f3=1.5, k1=1.5, k2=1.5) command = Command(pose=pose, velocity=vel) if self.live_update.isChecked() and self.isVisible(): self.command_pub.publish(command) if self.record_button.text() == "Stop Recording": self.listCommand.append(command)
import sys, rospy, cv2 from reflex_msgs.msg import PoseCommand from cibr_img_processing.msg import Ints global cmd, is_grasping is_grasping = False cmd = PoseCommand() cmd.f1 = 0.0 cmd.f2 = 0.0 cmd.f3 = 0.0 def vission_cb(data): global cmd cmd.preshape = (0.0 if data[0] else 2.0) if not is_grasping: command_pub.publish(cmd) def handy(): global cmd if not is_grasping: cmd.f1 = 0.8 cmd.f2 = 0.8 cmd.f3 = 0.8 command_pub.publish(cmd) is_grasping = True print("Grasping") else: cmd.f1 = 0.0 cmd.f2 = 0.0 cmd.f3 = 0.0
def main(): ################################################################################################################## rospy.init_node('ExampleHandNode') # Services can automatically call hand calibration calibrate_fingers = rospy.ServiceProxy( '/reflex_takktile/calibrate_fingers', Empty) calibrate_tactile = rospy.ServiceProxy( '/reflex_takktile/calibrate_tactile', Empty) # Services can set tactile thresholds and enable tactile stops enable_tactile_stops = rospy.ServiceProxy( '/reflex_takktile/enable_tactile_stops', Empty) disable_tactile_stops = rospy.ServiceProxy( '/reflex_takktile/disable_tactile_stops', Empty) set_tactile_threshold = rospy.ServiceProxy( '/reflex_takktile/set_tactile_threshold', SetTactileThreshold) # This collection of publishers can be used to command the hand command_pub = rospy.Publisher('/reflex_takktile/command', Command, queue_size=1) pos_pub = rospy.Publisher('/reflex_takktile/command_position', PoseCommand, queue_size=1) vel_pub = rospy.Publisher('/reflex_takktile/command_velocity', VelocityCommand, queue_size=1) force_pub = rospy.Publisher('/reflex_takktile/command_motor_force', ForceCommand, queue_size=1) # Constantly capture the current hand state rospy.Subscriber('/reflex_takktile/hand_state', Hand, hand_state_cb) ################################################################################################################## # Calibrate the fingers (make sure hand is opened in zero position) raw_input("== When ready to calibrate the hand, press [Enter]\n") calibrate_fingers() raw_input("...\n") while True: demo_type = input( "1: Wiggle, 2: Preshape, 3: Guarded, 4:Velocity, 5:Force, 6: Enable TakkTile, 7: Disable TakkTile, 8: Calibrate Fingers 9:Exit\n" ) if demo_type == 1: # Demonstration of position control wiggle_cycles = input("How many times would you like to wiggle?\n") raw_input( "== When ready to wiggle fingers with position control, hit [Enter]\n" ) for i in range(wiggle_cycles): pos_pub.publish( PoseCommand(f1=3.5, f2=3.5, f3=3.5, preshape=0.0)) rospy.sleep(1.0) pos_pub.publish(PoseCommand()) rospy.sleep(1.0) if demo_type == 2: # Demonstration of preshape joint preshape_cycles = input( "How many times would you like to test the preshape?\n") raw_input("== When ready to test preshape joint, hit [Enter]\n") for i in range(preshape_cycles): pos_pub.publish(PoseCommand(preshape=1.57)) rospy.sleep(1.0) pos_pub.publish( PoseCommand(f1=3.0, f2=3.0, f3=0, preshape=1.57)) rospy.sleep(2.0) pos_pub.publish(PoseCommand()) rospy.sleep(2.0) if demo_type == 3: # Demonstration of tactile feedback and setting sensor thresholds raw_input( "== When ready to calibrate tactile sensors and close until contact, hit [Enter]\n" ) calibrate_tactile() enable_tactile_stops() f1 = FingerPressure([10, 10, 10, 10, 10, 10, 10, 10, 1000]) f2 = FingerPressure([15, 15, 15, 15, 15, 15, 15, 15, 1000]) f3 = FingerPressure([20, 20, 20, 20, 20, 20, 20, 20, 1000]) threshold = SetTactileThresholdRequest([f1, f2, f3]) set_tactile_threshold(threshold) vel_pub.publish( VelocityCommand(f1=1.0, f2=1.0, f3=1.0, preshape=0.0)) rospy.sleep(3.0) pos_pub.publish(PoseCommand()) disable_tactile_stops() if demo_type == 4: raw_input("== When ready to test velocity control, hit [Enter]\n") vel_pub.publish( VelocityCommand(f1=1.0, f2=2.0, f3=3.0, preshape=0.0)) rospy.sleep(5.0) pos_pub.publish(PoseCommand()) rospy.sleep(1.0) if demo_type == 5: raw_input("== When ready to test Force control, hit [Enter]\n") force_pub.publish( ForceCommand(f1=200, f2=200, f3=200, preshape=0.0)) rospy.sleep(5.0) pos_pub.publish(PoseCommand()) rospy.sleep(1.0) if demo_type == 6: enable_tactile_stops() print("TakkTile Sensors Enabled") if demo_type == 7: disable_tactile_stops() print("TakkTile Sensors Disabled") if demo_type == 8: calibrate_fingers() print("Finger Calibration Complete") if demo_type == 9: break
def handleButtonHome(self): poseTarget = PoseCommand(f1=0.0, f2=0.0, f3=0.0, preshape=0.0) self.command_pub.publish(poseTarget) print "Home Button Click"
def main(): ################################################################################################################## rospy.init_node('ExampleHandNode') # sensor init # initial() # Services can automatically call hand calibration calibrate_fingers = rospy.ServiceProxy( '/reflex_takktile/calibrate_fingers', Empty) calibrate_tactile = rospy.ServiceProxy( '/reflex_takktile/calibrate_tactile', Empty) # Services can set tactile thresholds and enable tactile stops enable_tactile_stops = rospy.ServiceProxy( '/reflex_takktile/enable_tactile_stops', Empty) disable_tactile_stops = rospy.ServiceProxy( '/reflex_takktile/disable_tactile_stops', Empty) set_tactile_threshold = rospy.ServiceProxy( '/reflex_takktile/set_tactile_threshold', SetTactileThreshold) # This collection of publishers can be used to command the hand command_pub = rospy.Publisher('/reflex_takktile/command', Command, queue_size=1) pos_pub = rospy.Publisher('/reflex_takktile/command_position', PoseCommand, queue_size=1) vel_pub = rospy.Publisher('/reflex_takktile/command_velocity', VelocityCommand, queue_size=1) force_pub = rospy.Publisher('/reflex_takktile/command_motor_force', ForceCommand, queue_size=1, latch=True) # Constantly capture the current hand state rospy.Subscriber('/reflex_takktile/hand_state', Hand, hand_state_cb) ################################################################################################################## # # Calibrate the fingers (make sure hand is opened in zero position) # raw_input("== When ready to calibrate the hand, press [Enter]\n") # calibrate_fingers() # raw_input("... [Enter]\n") # ################################################################################################################## # Demonstration of position control rospy.sleep(1.0) pos_pub.publish(PoseCommand(f1=2.3, f2=2.3, f3=2.3, preshape=0.0)) rospy.sleep(2.0) rospy.loginfo("Ready to use the sensor control") while not rospy.is_shutdown(): #check() #def check(): # posi_pub = rospy.Publisher('/reflex_takktile/command_position', PoseCommand, queue_size=1) # # #Position control part # rospy.Subscriber('chatter', Float64, callback) # rospy.loginfo(rospy.get_caller_id() + "in the loop %s", cap) # if cap > 10000.0: # pos_pub.publish(PoseCommand(f1=1.0, f2=1.0, f3=2.0, preshape=1.5)) # rospy.sleep(1.0) # else: # pos_pub.publish(PoseCommand(f1=1.0, f2=1.0, f3=3.0, preshape=0.0)) # rospy.sleep(1.0) #force control rospy.Subscriber('capacitance', Float64, cap_cb) rospy.Subscriber('mean', Float64, mean_cb) rospy.loginfo(rospy.get_caller_id() + "in the loop %s", cap) if cap - mean > 1 and cap - mean < 2: force_pub.publish(ForceCommand(f3=50.0)) rospy.sleep(1.0) elif cap - mean > 2 and cap - mean < 3: force_pub.publish(ForceCommand(f3=75.0)) rospy.sleep(1.0) elif cap - mean > 3: force_pub.publish(ForceCommand(f3=100.0)) rospy.sleep(1.0) elif cap - mean <= -1: pos_pub.publish(PoseCommand(f1=0.0, f2=0.0, f3=0.0, preshape=0.0)) rospy.sleep(2.0) pos_pub.publish(PoseCommand(f1=2.3, f2=2.3, f3=2.3, preshape=0.0)) rospy.sleep(1.0)
def main(): ################################################################################################################## rospy.init_node('ExampleHandNode') # Services can automatically call hand calibration prefix = '/reflex_hand2' calibrate_fingers = rospy.ServiceProxy(prefix + '/calibrate_fingers', Empty) # This collection of publishers can be used to command the hand command_pub = rospy.Publisher(prefix + '/command', Command, queue_size=1) pos_pub = rospy.Publisher(prefix + '/command_position', PoseCommand, queue_size=1) vel_pub = rospy.Publisher(prefix + '/command_velocity', VelocityCommand, queue_size=1) # force_pub = rospy.Publisher('/reflex_sf/command_motor_force', ForceCommand, queue_size=1) # Constantly capture the current hand state rospy.Subscriber(prefix + '/hand_state', Hand, hand_state_cb) ################################################################################################################## # Calibrate the fingers (make sure hand is opened in zero position) #raw_input("== When ready to calibrate the hand, press [Enter]\n") #print('Go to the window where reflex_sf was run, you will see the calibration prompts') #calibrate_fingers() #raw_input("...\n") ################################################################################################################## # Demonstration of position control raw_input( "== When ready to wiggle fingers with position control, hit [Enter]\n") for i in range(20): setpoint = (-cos(i / 5.0) + 1) * 1.75 pos_pub.publish( PoseCommand(f1=setpoint, f2=setpoint, f3=setpoint, preshape=0.0)) rospy.sleep(0.25) raw_input("...\n") ################################################################################################################## # Demonstration of preshape joint raw_input("== When ready to test preshape joint, hit [Enter]\n") pos_pub.publish(PoseCommand(preshape=1.57)) rospy.sleep(2.0) pos_pub.publish(PoseCommand()) rospy.sleep(2.0) raw_input("...\n") ################################################################################################################## # Demonstration of velocity control - variable closing speed raw_input( "== When ready to open and close fingers with velocity control, hit [Enter]\n" ) for i in range(3): pos_pub.publish(PoseCommand(f1=i, f2=i, f3=i, preshape=0.0)) rospy.sleep(2.0) setpoint = 5.0 - (i * 2.25) vel_pub.publish( VelocityCommand(f1=setpoint, f2=setpoint, f3=setpoint, preshape=0.0)) rospy.sleep(7.0 - setpoint) raw_input("...\n") pos_pub.publish(PoseCommand()) ################################################################################################################## # Demonstration of blended control - asymptotic approach to goal - uses hand_state raw_input( "== When ready to approach target positions with blended control, hit [Enter]\n" ) pose = PoseCommand(f1=3.5, f2=2.25, f3=1.0, preshape=0.0) velocity = VelocityCommand() for i in range(1, 5): velocity.f1 = round(pose.f1 - hand_state.motor[0].joint_angle, 1) + 0.5 velocity.f2 = round(pose.f2 - hand_state.motor[1].joint_angle, 1) + 0.5 velocity.f3 = round(pose.f3 - hand_state.motor[2].joint_angle, 1) + 0.5 command_pub.publish(Command(pose, velocity)) rospy.sleep(0.75) raw_input("...\n")
def move_hand(f1, f2, f3, preshape, v1, v2, v3, vp): pose = PoseCommand(f1, f2, f3, preshape) velocity = VelocityCommand(v1, v2, v3, vp) command_pub.publish(Command(pose, velocity))
def move_pos(f1=0, f2=0, f3=0, preshape=0): pos_pub.publish(PoseCommand(f1, f2, f3, preshape))
def initUI(self): slider_min = 0 slider_max = 200 spin_box_step = 0.1 spin_box_init = 0.5 spin_box_max = 2 speed_label = "Rad/Sec" ################## Position Control GUI ######################################################################## ######### Automatic Hand Move ################################################################################### #TODO: this may happen too fast? need to test on hand, make smooth like buddah #Synchronize selected fingers self.hbox_update_tick = QHBoxLayout() self.live_update = QCheckBox("Turn on live update") self.hbox_update_tick.addWidget(self.live_update) self.hbox_update_tick.addStretch() #Fingers: slider range 0 -> 200 #Finger 1 row (F1) self.finger_label_1 = QLabel("Goal for f1") self.finger_slider_1 = QSlider(1) self.finger_slider_1.setMinimum(slider_min) self.finger_slider_1.setMaximum(slider_max) self.finger_slider_1.setValue(slider_min) self.value_slider_1 = QLabel(str(slider_min)) self.value_slider_1.setMaximumSize(80, 20) self.f1_speed = QDoubleSpinBox() self.f1_speed.setSingleStep(spin_box_step) self.f1_speed.setValue(spin_box_init) self.f1_speed.setMaximum(spin_box_max) self.hbox_f1 = QHBoxLayout() self.hbox_f1.addWidget(self.finger_slider_1) self.hbox_f1.addWidget(self.value_slider_1) self.hbox_f1.addWidget(self.f1_speed) self.hbox_f1.addWidget(QLabel(speed_label)) #Finger 2 row (F2) self.finger_label_2 = QLabel("Goal for f2") self.finger_slider_2 = QSlider(1) self.finger_slider_2.setMinimum(slider_min) self.finger_slider_2.setMaximum(slider_max) self.finger_slider_2.setValue(slider_min) self.value_slider_2 = QLabel(str(slider_min)) self.value_slider_2.setMaximumSize(80, 20) self.f2_speed = QDoubleSpinBox() self.f2_speed.setSingleStep(spin_box_step) self.f2_speed.setValue(spin_box_init) self.f2_speed.setMaximum(spin_box_max) self.hbox_f2 = QHBoxLayout() self.hbox_f2.addWidget(self.finger_slider_2) self.hbox_f2.addWidget(self.value_slider_2) self.hbox_f2.addWidget(self.f2_speed) self.hbox_f2.addWidget(QLabel(speed_label)) #Finger 3 row (F3) self.finger_label_3 = QLabel("Goal for f3") self.finger_slider_3 = QSlider(1) self.finger_slider_3.setMinimum(slider_min) self.finger_slider_3.setMaximum(slider_max) self.finger_slider_3.setValue(slider_min) self.value_slider_3 = QLabel(str(slider_min)) self.value_slider_3.setMaximumSize(80, 20) self.f3_speed = QDoubleSpinBox() self.f3_speed.setSingleStep(spin_box_init) self.f3_speed.setValue(spin_box_init) self.f3_speed.setMaximum(spin_box_max) self.hbox_f3 = QHBoxLayout() self.hbox_f3.addWidget(self.finger_slider_3) self.hbox_f3.addWidget(self.value_slider_3) self.hbox_f3.addWidget(self.f3_speed) self.hbox_f3.addWidget(QLabel(speed_label)) #Reflex SF hand distance between fingers (k1) self.finger_label_4 = QLabel("Distance between fingers 1 and 2") self.finger_slider_4 = QSlider(1) self.finger_slider_4.setMinimum(slider_min) self.finger_slider_4.setMaximum(slider_max) self.finger_slider_4.setValue(slider_min) #initialize at position 0 self.value_slider_4 = QLabel(str(slider_min)) self.value_slider_4.setMaximumSize(80, 20) self.f4_speed = QDoubleSpinBox() self.f4_speed.setSingleStep(spin_box_step) self.f4_speed.setValue(spin_box_init) self.f4_speed.setMaximum(spin_box_max) self.hbox_f4 = QHBoxLayout() self.hbox_f4.addWidget(self.finger_slider_4) self.hbox_f4.addWidget(self.value_slider_4) self.hbox_f4.addWidget(self.f4_speed) self.hbox_f4.addWidget(QLabel(speed_label)) #Reflex SF hand thumb rotation (k2) #set range to -100 to 100 since it is easier for user to conceptualize self.finger_label_5 = QLabel("Thumb Rotation") self.finger_slider_5 = QSlider(1) self.finger_slider_5.setMinimum(-100) self.finger_slider_5.setMaximum(100) self.finger_slider_5.setValue(0) #set initial value to center position self.value_slider_5 = QLabel(str(slider_min)) self.value_slider_5.setMaximumSize(80, 20) self.f5_speed_lbl = QLabel(speed_label) self.f5_speed = QDoubleSpinBox() self.f5_speed.setSingleStep(spin_box_step) self.f5_speed.setValue(spin_box_init) self.f5_speed.setMaximum(spin_box_max) self.hbox_f5 = QHBoxLayout() self.hbox_f5.addWidget(self.finger_slider_5) self.hbox_f5.addWidget(self.value_slider_5) self.hbox_f5.addWidget(self.f5_speed) self.hbox_f5.addWidget(self.f5_speed_lbl) ########## Coupling Row ################################################################################### #Synchronize selected fingers self.coupling_label = QLabel("Coupling") self.hbox_tick = QHBoxLayout() self.tick_f1 = QCheckBox("F1") self.tick_f2 = QCheckBox("F2") self.tick_f3 = QCheckBox("F3") #only display finger 4 for the Reflex SF hand self.tick_f4 = QCheckBox("F4") self.tick_f4.setHidden(True) self.hbox_tick.addWidget(self.tick_f1) self.hbox_tick.addWidget(self.tick_f2) self.hbox_tick.addWidget(self.tick_f3) self.hbox_tick.addWidget(self.tick_f4) self.hbox_tick.addStretch() ########### Command Row Button ############################################################################ self.command_label = QLabel("Manual Command Hand") self.go_button = QPushButton("Go to set values") self.re_button = QPushButton("Reset Goal Values") self.home_button = QPushButton("Finger Home") self.hbox_command = QHBoxLayout() self.hbox_command.addWidget(self.go_button) self.hbox_command.addWidget(self.re_button) self.hbox_command.addWidget(self.home_button) ########### Select Active Hand ############################################################################ self.combo_label = QLabel("Targeted Device") self.combo = QComboBox(self) self.combo.addItem("ReflexSF") self.combo.addItem("Soft Hand") ############ Manage Waypoint List ############################################################################################## self.listCommand = [] self.grasplist = [] self.filename = [] #initial point at conceptual home position (thumb in center position) self.pose0 = PoseCommand(f1=0.0, f2=0.0, f3=0.0, k1=0.0, k2=1.0) self.velocity0 = VelocityCommand(f1=1, f2=1, f3=1, k1=1, k2=1) command0 = Command(pose=self.pose0, velocity=self.velocity0) self.listCommand.append(command0) #Display waypoints and files self.listWidget = QListWidget() self.fileListWidget = QListWidget() self.listWidget.installEventFilter( self) #######################################################3 self.populate_filelist() self.populate_commandlist() self.fileslabel = QLabel("Grasp Files") self.listlabel = QLabel("List waypoint") # Options for File List self.file_control = QHBoxLayout() self.save_file_button = QPushButton("Save to File") self.file_load_button = QPushButton("Load File into Waypoint List") self.file_execute_button = QPushButton("Execute File") self.file_control.addWidget(self.file_load_button) self.file_control.addWidget(self.save_file_button) self.file_control.addWidget(self.file_execute_button) #List Control self.list_control_label = QLabel("Waypoint Control") self.list_control_save_button = QPushButton("Add Waypoint") self.list_control_delay_button = QPushButton("Add Delay") self.list_control_execute_waypoints = QPushButton("Execute Waypoints") self.list_control_save_grasp = QPushButton("Save Grasp") self.list_control_delete_all = QPushButton("Delete All Waypoints") self.list_control = QHBoxLayout() self.list_control.addWidget(self.list_control_save_button) self.list_control.addWidget(self.list_control_delay_button) self.list_control.addWidget(self.list_control_execute_waypoints) self.list_control.addWidget(self.list_control_delete_all) #calibration self.calibration = QHBoxLayout() self.calibrate_button = QPushButton("Calibrate Hand") self.calibration.addWidget(self.calibrate_button) ############ Adding Sections to GUI #################################################### #using the buttons defined above to create the GUI itself self.fbox = QFormLayout() self.fbox.addRow(QLabel(""), self.hbox_update_tick) self.fbox.addRow(self.finger_label_1, self.hbox_f1) self.fbox.addRow(self.finger_label_2, self.hbox_f2) self.fbox.addRow(self.finger_label_3, self.hbox_f3) self.fbox.addRow(self.finger_label_4, self.hbox_f4) self.fbox.addRow(self.finger_label_5, self.hbox_f5) self.fbox.addRow(self.coupling_label, self.hbox_tick) self.fbox.addRow(self.command_label, self.hbox_command) self.fbox.addRow(self.listlabel, self.listWidget) self.fbox.addRow(self.list_control_label, self.list_control) self.fbox.addRow(self.fileslabel, self.fileListWidget) self.fbox.addRow(QLabel(""), self.file_control) self.fbox.addRow(QLabel(""), self.calibration) self.fbox.addRow(self.combo_label, self.combo) # Connect signal when slider change to function respectively to change value of label self.finger_slider_1.valueChanged.connect(lambda: self.sliderMoved(1)) self.finger_slider_2.valueChanged.connect(lambda: self.sliderMoved(2)) self.finger_slider_3.valueChanged.connect(lambda: self.sliderMoved(3)) self.finger_slider_4.valueChanged.connect(lambda: self.sliderMoved(4)) self.finger_slider_5.valueChanged.connect(lambda: self.sliderMoved(5)) # Add connect signal to Button Go, Cancel and Reset self.go_button.clicked.connect(self.handleButtonGo) self.home_button.clicked.connect(self.handleButtonHome) self.re_button.clicked.connect(self.handleButtonReset) #Add connect signal when combo box changes self.combo.currentIndexChanged.connect(self.handleHandSelectChange) self.list_control_save_button.clicked.connect( self.handle_list_control_save_button) self.list_control_delay_button.clicked.connect(self.handle_add_delay) self.list_control_execute_waypoints.clicked.connect( self.handle_execute_waypoints) self.list_control_save_grasp.clicked.connect( self.handle_grasp_save_button) self.list_control_delete_all.clicked.connect(self.handle_delete_all) self.file_execute_button.clicked.connect( self.handle_run_existing_grasp_button) self.file_load_button.clicked.connect(self.handle_add_file_waypoints) self.save_file_button.clicked.connect(self.handle_grasp_save_button) self.calibrate_button.clicked.connect(self.handle_calibrate_widget) ######### Set up window ################################################################################### #Set the widget to layout and show the widget self.setLayout(self.fbox)
def main(): setupRospy() #SERVERCODE###############3 s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) # ports 1990-1995 can be used) s.bind(('', 1993)) s.listen(1) print("#" * 50 + "\n\t\tREADY TO RUMBLE\n" + "#" * 50) print("\nTACTILE-STOPS ENABLED BY DEFAULT\n\n") enable_tactile_stops() while True: conn, addr = s.accept() bytes_received = conn.recv(100) stripped = bytes_received.strip() split = stripped.split() cmnd = str(split[0]) if not ('STATS' == cmnd): print(stripped) if 'POSE' in str(cmnd): value = [float(x) for x in split[1:]] move_hand(value[0], value[1], value[2], value[3], value[4], value[5], value[6], value[7]) conn.sendall('POSE-DONE\r\n') elif 'STATS' in str(cmnd): conn.sendall(get_pretty_stats()) elif 'VEL' in str(cmnd): command = str(split[1]) extra_speed = split[2] velocity_controlled(command, extra_speed) conn.sendall('VEL-DONE\r\n') elif 'THRESH' in str(cmnd): thresholds = [float(x) for x in split[1:]] set_thresholds(thresholds[0], thresholds[1], thresholds[2], thresholds[3], thresholds[4], thresholds[5]) conn.sendall('THRESH-DONE\r\n') elif 'GOTOLIMIT' in str(cmnd): go_to_limit() conn.sendall('CLOSED-LIMIT\r\n') elif 'RESET' in str(cmnd): move_pos() conn.sendall('RESET-DONE\r\n') elif 'CALT' in str(cmnd): calibrate_tactile() conn.sendall('TACTILE-CALIBRATED\r\n') elif 'CALF' in str(cmnd): calibrate_fingers() conn.sendall('FINGERS-CALIBRATED\r\n') elif 'TACON' in str(cmnd): enable_tactile_stops() conn.sendall('TACTILE-ENABLED\r\n') elif 'TACOFF' in str(cmnd): disable_tactile_stops() conn.sendall('TACTILE-DISABLED\r\n') elif 'QUIT' in str(cmnd): pos_pub.publish(PoseCommand()) conn.sendall('QUIT-DONE\r\n') break elif 'SERVER' in str(cmnd): conn.sendall('SERVER-CONNECTED\r\n') else: conn.sendall('UNKNOWN-CMD\r\n') s.close()
def main(): ################################################################################################################## rospy.init_node('ExampleHandNode') # Services can automatically call hand calibration calibrate_fingers = rospy.ServiceProxy( '/reflex_takktile/calibrate_fingers', Empty) calibrate_tactile = rospy.ServiceProxy( '/reflex_takktile/calibrate_tactile', Empty) # Services can set tactile thresholds and enable tactile stops enable_tactile_stops = rospy.ServiceProxy( '/reflex_takktile/enable_tactile_stops', Empty) disable_tactile_stops = rospy.ServiceProxy( '/reflex_takktile/disable_tactile_stops', Empty) set_tactile_threshold = rospy.ServiceProxy( '/reflex_takktile/set_tactile_threshold', SetTactileThreshold) # This collection of publishers can be used to command the hand command_pub = rospy.Publisher('/reflex_takktile/command', Command, queue_size=1) pos_pub = rospy.Publisher('/reflex_takktile/command_position', PoseCommand, queue_size=1) vel_pub = rospy.Publisher('/reflex_takktile/command_velocity', VelocityCommand, queue_size=1) force_pub = rospy.Publisher('/reflex_takktile/command_motor_force', ForceCommand, queue_size=1) # Constantly capture the current hand state rospy.Subscriber('/reflex_takktile/hand_state', Hand, hand_state_cb) ################################################################################################################## # Calibrate the fingers (make sure hand is opened in zero position) raw_input("== When ready to calibrate the hand, press [Enter]\n") calibrate_fingers() raw_input("... [Enter]\n") ################################################################################################################## # Demonstration of position control raw_input( "== When ready to wiggle fingers with position control, hit [Enter]\n") for i in range(100): setpoint = (-cos(i / 15.0) + 1) * 1.75 pos_pub.publish( PoseCommand(f1=setpoint, f2=setpoint, f3=setpoint, preshape=0.0)) rospy.sleep(0.1) raw_input("... [Enter]\n") pos_pub.publish(PoseCommand()) ################################################################################################################## # Demonstration of preshape joint raw_input("== When ready to test preshape joint, hit [Enter]\n") pos_pub.publish(PoseCommand(preshape=1.57)) rospy.sleep(2.0) pos_pub.publish(PoseCommand()) rospy.sleep(2.0) raw_input("... [Enter]\n") ################################################################################################################## # Demonstration of velocity control - variable closing speed raw_input( "== When ready to open and close fingers with velocity control, hit [Enter]\n" ) vel_pub.publish(VelocityCommand(f1=3.0, f2=2.0, f3=1.0, preshape=0.0)) rospy.sleep(4.0) vel_pub.publish(VelocityCommand(f1=-1.0, f2=-2.0, f3=-3.0, preshape=0.0)) rospy.sleep(4.0) raw_input("... [Enter]\n") pos_pub.publish(PoseCommand()) ################################################################################################################## # Demonstration of blended control - asymptotic approach to goal - uses hand_state raw_input( "== When ready to approach target positions with blended control, hit [Enter]\n" ) pose = PoseCommand(f1=3.5, f2=2.25, f3=1.0, preshape=0.0) velocity = VelocityCommand() for i in range(1, 5): velocity.f1 = round(pose.f1 - hand_state.motor[0].joint_angle, 1) + 0.25 velocity.f2 = round(pose.f2 - hand_state.motor[1].joint_angle, 1) + 0.25 velocity.f3 = round(pose.f3 - hand_state.motor[2].joint_angle, 1) + 0.25 command_pub.publish(Command(pose, velocity)) rospy.sleep(0.4) raw_input("... [Enter]\n") pos_pub.publish(PoseCommand()) ################################################################################################################## # # Demonstration of force control - square wave raw_input("== When ready to feel variable force control, hit [Enter]\n") raw_input( "== Putting your arm or hand in the hand will allow you to feel the effect [Enter]\n" ) pos_pub.publish(PoseCommand(f1=1.5, f2=1.5, f3=1.5, preshape=0.0)) rospy.sleep(2.0) print("Tightening finger 1") force_pub.publish(ForceCommand(f1=300.0)) rospy.sleep(5.0) print("Partially loosen finger 1") force_pub.publish(ForceCommand(f1=100.0)) rospy.sleep(1.0) print("Tightening finger 2") force_pub.publish(ForceCommand(f2=300.0)) rospy.sleep(5.0) print("Partially loosen finger 2") force_pub.publish(ForceCommand(f2=100.0)) rospy.sleep(1.0) print("Tightening finger 3") force_pub.publish(ForceCommand(f3=300.0)) rospy.sleep(5.0) print("Partially loosen finger 3") force_pub.publish(ForceCommand(f3=100.0)) rospy.sleep(1.0) vel_pub.publish(VelocityCommand(f1=-5.0, f2=-5.0, f3=-5.0, preshape=0.0)) rospy.sleep(2.0) raw_input("... [Enter]\n") ################################################################################################################## # Demonstration of tactile feedback and setting sensor thresholds raw_input( "== When ready to calibrate tactile sensors and close until contact, hit [Enter]\n" ) calibrate_tactile() enable_tactile_stops() f1 = FingerPressure([10, 10, 10, 10, 10, 10, 10, 10, 1000]) f2 = FingerPressure([15, 15, 15, 15, 15, 15, 15, 15, 1000]) f3 = FingerPressure([20, 20, 20, 20, 20, 20, 20, 20, 1000]) threshold = SetTactileThresholdRequest([f1, f2, f3]) set_tactile_threshold(threshold) vel_pub.publish(VelocityCommand(f1=1.0, f2=1.0, f3=1.0, preshape=0.0)) rospy.sleep(3.0) raw_input("... [Enter]\n") pos_pub.publish(PoseCommand()) disable_tactile_stops()