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 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 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('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("...\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 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()
