def close(self):
"""
A complex model of what Romano dictates to be contact
"""
print("In Close stage")
F_d_1, F_d_2, F_s = self.reader.get_forces()
F_d = (F_d_1 + F_d_2) / 2
F_d_dist, F_s_dist = self.reader.get_force_disturbances()
# Get accelerations from KUKA arm wrench?
# We can use this more complicated setup, or simply use the Reflex tactile stops
d_contact = F_d > _FLIMIT or F_d_dist > _DLIMIT
s_contact = F_s > _FLIMIT or F_s_dist > _DLIMIT
vel_cmd = VelocityCommand()
while not (d_contact and s_contact):
F_d_1, F_d_2, F_s = self.reader.get_forces()
F_d = (F_d_1 + F_d_2) / 2
F_d_dist, F_s_dist = self.reader.get_force_disturbances()
# Accelerations
d_contact = F_d > _FLIMIT or F_d_dist > _DLIMIT
s_contact = F_s > _FLIMIT or F_s_dist > _DLIMIT
# Move with desired velocity
self.vel_pub()
self.rate.sleep()
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 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 getVelocity(self):
"""
Return the current value of the velocity input box
"""
val1 = float(self.f1_speed.value())
val2 = float(self.f2_speed.value())
val3 = float(self.f3_speed.value())
val4 = float(self.f4_speed.value())
val5 = float(self.f5_speed.value())
velocity = VelocityCommand(f1=val1, f2=val2, f3=val3, k1=val4, k2=val5)
return velocity
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 velocity_controlled(command, extra):
global negate
print(negate)
constant_speed = 0.2
set_speed = constant_speed + float(extra)
stop_speed = 0.00
if command == 'CLOSE':
negate = 1
v1 = set_speed
v2 = set_speed
v3 = set_speed
preshape = stop_speed
vel_pub.publish(VelocityCommand(v1, v2, v3, preshape))
elif command == 'OPEN':
negate = -1
v1 = -set_speed
v2 = -set_speed
v3 = -set_speed
preshape = stop_speed
vel_pub.publish(VelocityCommand(v1, v2, v3, preshape))
elif command == 'SPLIT':
negate = 0
v1 = stop_speed
v2 = stop_speed
v3 = stop_speed
preshape = set_speed
vel_pub.publish(VelocityCommand(v1, v2, v3, preshape))
elif command == 'MERGE':
negate = 0
v1 = stop_speed
v2 = stop_speed
v3 = stop_speed
preshape = -set_speed
vel_pub.publish(VelocityCommand(v1, v2, v3, preshape))
elif command == 'STOP':
stop_hand(negate)
else:
print("WRONG COMMAND")
def _close_hand(self):
self._zero_tactile()
self._enable_tactile_stops()
# Close the hand at constant velocity until it contacts object
vel_cmd = VelocityCommand()
vel_cmd.f1 = 1.0
vel_cmd.f2 = 1.0 # bad finger
vel_cmd.f3 = 1.0
vel_cmd.preshape = 0.0
for i in range(10):
self.vel_cmd_pub.publish(vel_cmd)
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 vel_pub(self, event=None, v=None):
"""
While the internal representation holds to Romano's conventions of
negative velocity => closing, the reflex hand has the opposite convention,
so this function exists to abstract that detail away from the rest of
the functions. This way, we can remain as close as possible to Romano's
representation.
"""
if v is None:
v = self.v_des
vel_cmd = VelocityCommand()
# Potentially halve each velocity, since the closure is coming from both directions?
vel_cmd.f1 = -v
vel_cmd.f2 = -v
vel_cmd.f3 = -v
vel_cmd.preshape = 0.0
self.vel_cmd_pub.publish(vel_cmd)
def open(self):
#self.pos_pub.publish(VelocityCommand(f1 = 0, f2 = 0, f3 = 0, preshape = 0.0))
self.pos_pub.publish(VelocityCommand(f1=0, f2=0, f3=0, preshape=0.0))
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 pointing(self):
#self.pos_pub.publish(VelocityCommand(f1 = 2, f2 = 3, f3 = 3, preshape = 0.0))
self.pos_pub.publish(VelocityCommand(f1=2, f2=0, f3=3, preshape=0.0))
def grasp(self):
#self.pos_pub.publish(VelocityCommand(f1 = 3, f2 = 3, f3 = 3, preshape = 0.0))
self.pos_pub.publish(VelocityCommand(f1=3, f2=0, f3=3, preshape=0.0))
def main():
##################################################################################################################
rospy.init_node('ExampleHandNode')
# Services can automatically call hand calibration
calibrate_fingers = rospy.ServiceProxy('/reflex_sf/calibrate_fingers', Empty)
# This collection of publishers can be used to command the hand
command_pub = rospy.Publisher('/reflex_sf/command', Command, queue_size=1)
pos_pub = rospy.Publisher('/reflex_sf/command_position', PoseCommand, queue_size=1)
vel_pub = rospy.Publisher('/reflex_sf/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('/reflex_sf/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(60):
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('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()
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(60):
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")
##################################################################################################################
# # Demonstration of force control - square wave
# # NEEDS TESTING
# 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)
# force_pub.publish(ForceCommand(f1=300.0))
# rospy.sleep(5.0)
# force_pub.publish(ForceCommand(f1=100.0))
# rospy.sleep(3.0)
# force_pub.publish(ForceCommand(f2=300.0))
# rospy.sleep(5.0)
# force_pub.publish(ForceCommand(f2=100.0))
# rospy.sleep(3.0)
# force_pub.publish(ForceCommand(f3=300.0))
# rospy.sleep(5.0)
# force_pub.publish(ForceCommand(f3=100.0))
# rospy.sleep(3.0)
# vel_pub.publish(VelocityCommand(f1=-5.0, f2=-5.0, f3=-5.0, preshape=0.0))
# rospy.sleep(2.0)
# raw_input("...\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)
velocity = VelocityCommand(f1=1.0, f2=1.0, f3=1.0, preshape=0.0)
rospy.sleep(3.0)
raw_input("...\n")
pos_pub.publish(PoseCommand())
disable_tactile_stops()
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 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_vel(v1, v2, v3, preshape=0):
vel_pub.publish(VelocityCommand(v1, v2, v3, preshape))
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 pre_grasp(self, s):
#self.pos_pub.publish(VelocityCommand(f1 = s * 5, f2 = s * 5 ,f3 = s * 5, preshape = 0.0))
self.pos_pub.publish(
VelocityCommand(f1=s * 5, f2=0, f3=s * 5, preshape=0.0))
