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()
