def getCurrCommand(self):
"""
Combine the pose and velocity inputs into a single command
"""
pose = self.getSliderPose()
vel = self.getVelocity()
return Command(pose=pose, velocity=vel)
def liveUpdateCntrl(self):
while (True):
if (self.isVisible() and self.live_update.isChecked()):
vel = VelocityCommand(f1=1, f2=1, f3=1, k1=1, k2=1)
pos = self.getSliderPose()
self.moveHandtoPose(Command(pose=pos, velocity=vel))
rospy.sleep(0.2)
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 handle_list_control_save_button(self):
"""
Save Current slider pose to waypoint list
"""
slider_pose = self.getSliderPose()
velocity0 = self.getVelocity()
self.addWaypoint(is_below=-1,
cmd=Command(pose=slider_pose, velocity=velocity0))
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 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 hand_control(self, _f1, _f2, _f3, _v):
hand = Command()
hand.pose.f1 = _f1
hand.pose.f2 = _f2
hand.pose.f3 = _f3
hand.pose.preshape = 0.0
hand.velocity.f1 = _v
hand.velocity.f2 = _v
hand.velocity.f3 = _v
hand.velocity.preshape = 2.0
print "Grasping..."
time.sleep(2.0)
rospy.loginfo(hand)
self.hand_pub.publish(hand)
print "Done..."
def hand_close(self):
hand = Command()
hand.pose.f1 = 2.2
hand.pose.f2 = 2.2
hand.pose.f3 = 2.2
hand.pose.preshape = 0.0
hand.velocity.f1 = 2.0
hand.velocity.f2 = 2.0
hand.velocity.f3 = 2.0
hand.velocity.preshape = 2.0
print "Closing hand..."
rospy.loginfo(hand)
time.sleep(2.0)
self.hand_pub.publish(hand)
print "Done..."
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)
def eventFilter(self, source, event):
"""
Add actions to context menu on waypoint items
"""
if (event.type() == QEvent.ContextMenu):
menu = QMenu(self)
#waypoint actions
waypointMenu = menu.addMenu("Add Waypoint")
addWaypointAbove = waypointMenu.addAction("Add Above")
addWaypointBelow = waypointMenu.addAction("Add Below")
#delay actions
delayMenu = menu.addMenu("Add Delay")
addDelayAbove = delayMenu.addAction("Add Above")
addDelayBelow = delayMenu.addAction("Add Below")
#general actions
deleteWaypoint = menu.addAction("Delete")
#execute actions
action = menu.exec_(event.globalPos())
slider_pose = self.getSliderPose()
vel = self.getVelocity()
des_cmd = Command(pose=slider_pose, velocity=vel)
if action == addWaypointAbove:
self.addWaypoint(is_below=False, cmd=des_cmd)
if action == addWaypointBelow:
self.addWaypoint(is_below=True, cmd=des_cmd)
if action == addDelayAbove:
delay = self.getDelay()
if delay != False: #if the delay was not canceled during prompt
self.addWaypoint(is_below=False, cmd=delay)
if action == addDelayBelow:
delay = self.getDelay()
if delay != False:
self.addWaypoint(is_below=True, cmd=delay)
if action == deleteWaypoint:
self.deleteWaypoint(self.listWidget.currentRow())
return QWidget.eventFilter(self, source, event)
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 main():
rospy.init_node("OTP_Experiment", disable_signals=True)
grasp_pub = rospy.Publisher("/right_hand/command", Command, queue_size=10)
hs = HumanSaver(1.66, 0.71)
face = head_wobbler.Wobbler()
base = move_base.MoveBase()
xdisplay_image.send_image("NeutralNEGray.jpg")
obs_sub = rospy.Subscriber("skeleton_data",
skeleton,
hs.callback,
queue_size=1)
time.sleep(2.0)
# hs.pointingDirection()
stand_right = promp.ProMP(
"src/handover/object_transfer_point/data/stand_right")
stand_45 = promp.ProMP("src/handover/object_transfer_point/data/stand_45")
stand_front = promp.ProMP(
"src/handover/object_transfer_point/data/stand_front")
stand_back = promp.ProMP(
"src/handover/object_transfer_point/data/stand_back")
sit_right = promp.ProMP(
"src/handover/object_transfer_point/data/sit_right")
sit_45 = promp.ProMP("src/handover/object_transfer_point/data/sit_45")
sit_front = promp.ProMP(
"src/handover/object_transfer_point/data/sit_front")
sit_back = promp.ProMP("src/handover/object_transfer_point/data/sit_back")
take = Command()
take.pose.f1 = 2.0
take.pose.f2 = 2.0
take.pose.f3 = 2.0
take.pose.preshape = 0.0
take.velocity.f1 = 3.0
take.velocity.f2 = 3.0
take.velocity.f3 = 3.0
take.velocity.preshape = 2.0
drop = Command()
drop.pose.f1 = 0.0
drop.pose.f2 = 0.0
drop.pose.f3 = 0.0
drop.pose.preshape = 0.0
drop.velocity.f1 = 3.0
drop.velocity.f2 = 3.0
drop.velocity.f3 = 3.0
drop.velocity.preshape = 2.0
grasp_pub.publish(drop)
face.set_neutral()
xdisplay_image.send_image("NeutralNEGray.jpg")
face.look_right()
text = raw_input("Start 1st case? (y/n)")
if text == 'n':
print "ok bye"
else:
xdisplay_image.send_image("NeutralNEGreen.jpg")
hs.traj_saver("stand_right")
hs.saver["stand_right"] = [
hs.height, hs.arm_length, hs.P_rw[0, 0], hs.P_rw[0, 1], hs.P_rw[0,
2]
]
obs_realtime = [[
hs.saver["stand_right"][2], hs.saver["stand_right"][3],
hs.saver["stand_right"][4]
]]
stand_right.test_promp(obs_realtime)
time.sleep(2.0)
grasp_pub.publish(take)
time.sleep(4.0)
stand_right.safe_right_dist()
stand_right.to_the_basket()
time.sleep(2.0)
grasp_pub.publish(drop)
time.sleep(2.0)
face.set_neutral()
stand_right.reset_right_hand()
text = raw_input("Safe to rotate? (y/n)")
if text == 'n':
print "ok bye"
else:
base.move_right()
xdisplay_image.send_image("NeutralNEGray.jpg")
face.look_45()
text = raw_input("Start 2rd case? (y/n)")
if text == 'n':
print "ok bye"
else:
xdisplay_image.send_image("NeutralNEGreen.jpg")
hs.traj_saver("stand_45")
hs.saver["stand_45"] = [
hs.height, hs.arm_length, hs.P_rw[0, 0], hs.P_rw[0, 1], hs.P_rw[0,
2]
]
obs_realtime = [[
hs.saver["stand_45"][2], hs.saver["stand_45"][3],
hs.saver["stand_45"][4]
]]
stand_45.test_promp(obs_realtime)
time.sleep(2.0)
grasp_pub.publish(take)
time.sleep(4.0)
stand_45.safe_45_dist()
stand_45.to_the_basket()
time.sleep(2.0)
grasp_pub.publish(drop)
time.sleep(2.0)
face.set_neutral()
stand_45.reset_right_hand()
text = raw_input("Safe to rotate? (y/n)")
if text == 'n':
print "ok bye"
else:
base.move_right()
xdisplay_image.send_image("NeutralNEYellow.jpg")
text = raw_input("Start 3rd case? (y/n)")
if text == 'n':
print "ok bye"
else:
xdisplay_image.send_image("NeutralNEGreen.jpg")
hs.traj_saver("stand_front")
hs.saver["stand_front"] = [
hs.height, hs.arm_length, hs.P_rw[0, 0], hs.P_rw[0, 1], hs.P_rw[0,
2]
]
obs_realtime = [[
hs.saver["stand_front"][2], hs.saver["stand_front"][3],
hs.saver["stand_front"][4]
]]
stand_front.test_promp(obs_realtime)
time.sleep(2.0)
grasp_pub.publish(take)
time.sleep(4.0)
stand_front.safe_front_dist()
stand_front.to_the_basket()
time.sleep(2.0)
grasp_pub.publish(drop)
time.sleep(2.0)
stand_front.reset_right_hand()
xdisplay_image.send_image("NeutralNERed.jpg")
text = raw_input("Start 4th case? (y/n)")
if text == 'n':
print "ok bye"
else:
xdisplay_image.send_image("NeutralNEGreen.jpg")
hs.traj_saver("stand_back")
hs.saver["stand_back"] = [
hs.height, hs.arm_length, hs.P_rw[0, 0], hs.P_rw[0, 1], hs.P_rw[0,
2]
]
hs.saver["stand_back_hand1"] = [
hs.height, hs.arm_length, hs.P_rw[0, 0], hs.P_rw[0, 1], hs.P_rw[0,
2]
]
hs.saver["stand_back_hand2"] = [
hs.height, hs.arm_length, hs.P_lw[0, 0], hs.P_lw[0, 1], hs.P_lw[0,
2]
]
obs_realtime = [[
hs.saver["stand_back"][2], hs.saver["stand_back"][3],
hs.saver["stand_back"][4]
]]
stand_back.test_promp(obs_realtime)
time.sleep(2.0)
grasp_pub.publish(take)
time.sleep(2.0)
stand_back.safe_front_dist()
stand_back.to_the_basket()
time.sleep(2.0)
grasp_pub.publish(drop)
time.sleep(2.0)
stand_back.reset_right_hand()
text = raw_input("Safe to rotate? (y/n)")
if text == 'n':
print "ok bye"
else:
base.move_left()
xdisplay_image.send_image("NeutralNEGray.jpg")
face.look_right()
text = raw_input("Start 5th case? (y/n)")
if text == 'n':
print "ok bye"
else:
xdisplay_image.send_image("NeutralNEGreen.jpg")
hs.traj_saver("sit_right")
hs.saver["sit_right"] = [
hs.height, hs.arm_length, hs.P_rw[0, 0], hs.P_rw[0, 1], hs.P_rw[0,
2]
]
obs_realtime = [[
hs.saver["sit_right"][2], hs.saver["sit_right"][3],
hs.saver["sit_right"][4]
]]
sit_right.test_promp(obs_realtime)
time.sleep(2.0)
grasp_pub.publish(take)
time.sleep(2.0)
sit_right.safe_right_dist()
sit_right.to_the_basket()
time.sleep(2.0)
grasp_pub.publish(drop)
time.sleep(2.0)
face.set_neutral()
sit_right.reset_right_hand()
text = raw_input("Safe to rotate? (y/n)")
if text == 'n':
print "ok bye"
else:
base.move_right()
xdisplay_image.send_image("NeutralNEGray.jpg")
face.look_45()
text = raw_input("Start 6th case? (y/n)")
if text == 'n':
print "ok bye"
else:
xdisplay_image.send_image("NeutralNEGreen.jpg")
hs.traj_saver("sit_45")
hs.saver["sit_45"] = [
hs.height, hs.arm_length, hs.P_rw[0, 0], hs.P_rw[0, 1], hs.P_rw[0,
2]
]
obs_realtime = [[
hs.saver["sit_45"][2], hs.saver["sit_45"][3], hs.saver["sit_45"][4]
]]
sit_45.test_promp(obs_realtime)
time.sleep(2.0)
grasp_pub.publish(take)
time.sleep(2.0)
sit_45.safe_45_dist()
sit_45.to_the_basket()
time.sleep(2.0)
grasp_pub.publish(drop)
time.sleep(2.0)
face.set_neutral()
sit_45.reset_right_hand()
text = raw_input("Safe to rotate? (y/n)")
if text == 'n':
print "ok bye"
else:
base.move_right()
xdisplay_image.send_image("NeutralNEYellow.jpg")
text = raw_input("Start 7th case? (y/n)")
if text == 'n':
print "ok bye"
else:
xdisplay_image.send_image("NeutralNEGreen.jpg")
hs.traj_saver("sit_front")
hs.saver["sit_front"] = [
hs.height, hs.arm_length, hs.P_rw[0, 0], hs.P_rw[0, 1], hs.P_rw[0,
2]
]
obs_realtime = [[
hs.saver["sit_front"][2], hs.saver["sit_front"][3],
hs.saver["sit_front"][4]
]]
sit_front.test_promp(obs_realtime)
time.sleep(2.0)
grasp_pub.publish(take)
time.sleep(2.0)
sit_front.safe_front_dist()
sit_front.to_the_basket()
time.sleep(2.0)
grasp_pub.publish(drop)
time.sleep(2.0)
sit_front.reset_right_hand()
xdisplay_image.send_image("NeutralNERed.jpg")
text = raw_input("Start 8th case? (y/n)")
if text == 'n':
print "ok bye"
else:
xdisplay_image.send_image("NeutralNEGreen.jpg")
hs.traj_saver("sit_back")
hs.saver["sit_back"] = [
hs.height, hs.arm_length, hs.P_rw[0, 0], hs.P_rw[0, 1], hs.P_rw[0,
2]
]
hs.saver["sit_back_hand1"] = [
hs.height, hs.arm_length, hs.P_rw[0, 0], hs.P_rw[0, 1], hs.P_rw[0,
2]
]
hs.saver["sit_back_hand2"] = [
hs.height, hs.arm_length, hs.P_lw[0, 0], hs.P_lw[0, 1], hs.P_lw[0,
2]
]
obs_realtime = [[
hs.saver["sit_back"][2], hs.saver["sit_back"][3],
hs.saver["sit_back"][4]
]]
sit_back.test_promp(obs_realtime)
time.sleep(2.0)
grasp_pub.publish(take)
time.sleep(2.0)
sit_back.safe_front_dist()
sit_back.to_the_basket()
time.sleep(2.0)
grasp_pub.publish(drop)
time.sleep(2.0)
sit_back.reset_right_hand()
with open(
"src/handover/object_transfer_point/data/subjects/human_" +
str(hs.subject) + "/human_wrist_" + str(hs.subject), "wb") as f:
pickle.dump(hs.saver, f)
sio.savemat(
"src/handover/object_transfer_point/data/subjects/human_" +
str(hs.subject) + "/human_wrist_" + str(hs.subject) + ".mat",
{'D': hs.saver})
xdisplay_image.send_image("JoyNEOrange.jpg")
text = raw_input("Safe to rotate? (y/n)")
if text == 'n':
print "ok bye"
else:
base.move_left()
print "Experiment Done!!"
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 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 runPromp(self):
grip = Command()
grip.pose.f1 = 0.0
grip.pose.f2 = 0.0
grip.pose.f3 = 0.0
grip.pose.preshape = 0.0
grip.velocity.f1 = 2.0
grip.velocity.f2 = 2.0
grip.velocity.f3 = 2.0
grip.velocity.preshape = 2.0
self.grasp_pub.publish(grip)
r = rospy.Rate(7)
traj_plot = np.empty((0, 7), dtype=float)
for j in range(len(self.kf["q_mean"][3])):
promp = []
for i in range(len(self.kf["q_mean"])):
promp.append(self.kf["q_mean"][i][j])
promp = np.array(promp[3:10])
traj_plot = np.append(traj_plot, np.matrix(promp), axis=0)
P = Float64MultiArray()
P.data = np.array(promp)
self.goal_pub.publish(P)
r.sleep()
np.savetxt("src/handover/scripts/traj2.csv", traj_plot, delimiter=",")
t = time.time()
tt = 0
r2 = rospy.Rate(7)
while tt < 1:
self.goal_pub.publish(P)
tt = time.time() - t
r2.sleep()
text = raw_input("Grab bottle? (Y/n)")
if text == 'n':
print "Ok. Going back."
else:
grip.pose.f1 = 2.0
grip.pose.f2 = 2.0
grip.pose.f3 = 2.0
grip.pose.preshape = 0.0
grip.velocity.f1 = 2.0
grip.velocity.f2 = 2.0
grip.velocity.f3 = 2.0
grip.velocity.preshape = 2.0
self.grasp_pub.publish(grip)
time.sleep(2.0)
text = raw_input("Give to human? (Y/n)")
if text == 'n':
print "Ok. Going back."
else:
otp = {
'right_s0': 0.6507913492603867,
'right_s1': -0.19941750242510378,
'right_w0': 1.992257548266181,
'right_w1': 0.043334957257762936,
'right_w2': -3.008136325043296,
'right_e0': 1.0496263541105944,
'right_e1': 0.4855049193657335
}
self.limb.move_to_joint_positions(otp, timeout=2.0)
grip.pose.f1 = 0.0
grip.pose.f2 = 0.0
grip.pose.f3 = 0.0
grip.pose.preshape = 0.0
grip.velocity.f1 = 2.0
grip.velocity.f2 = 2.0
grip.velocity.f3 = 2.0
grip.velocity.preshape = 2.0
self.grasp_pub.publish(grip)
time.sleep(2.0)
goback = {
'right_s0': -0.1902136176977913,
'right_s1': -0.24236896448589537,
'right_w0': 1.6171992456281974,
'right_w1': 0.4966262800779027,
'right_w2': -2.9931800123614134,
'right_e0': 0.9583544972314122,
'right_e1': 1.2133788032173622
}
self.limb.move_to_joint_positions(goback, timeout=2.0)
# promp = []
# for i in range(len(self.kf["q_mean"])):
# promp.append(self.kf["q_mean"][i][98])
# promp = np.array(promp[3:10])
# P = Float64MultiArray()
# P.data = promp
# self.goal_pub.publish(P)
text = raw_input("Restart motion? (Y/n)")
if text == 'n':
print "Ok. Not moving."
else:
init_angles = {
'right_s0': -1.6889128474618404,
'right_s1': 0.6412039693361029,
'right_w0': -0.1392087565006013,
'right_w1': 0.006519418348513008,
'right_w2': -3.0464858447404315,
'right_e0': 0.5829126993964572,
'right_e1': 1.0737865515197895
}
self.limb.move_to_joint_positions(init_angles, timeout=2.0)
text = raw_input("Run ProMP again? (Y/n)")
if text == 'n':
print "Ok. Not moving."
else:
self.runPromp()
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()