class GripperConnect(object):
"""
Connects wrist button presses to gripper open/close commands.
Uses the Navigator callback feature to make callbacks to connected
action functions when the button values change.
"""
def __init__(self, arm, lights=True):
"""
@type arm: str
@param arm: arm of gripper to control
@type lights: bool
@param lights: if lights should activate on cuff grasp
"""
self._arm = arm
# inputs
self._cuff = Cuff(limb=arm)
# connect callback fns to signals
self._lights = None
if lights:
self._lights = Lights()
self._cuff.register_callback(self._light_action,
'{0}_cuff'.format(arm))
try:
self._gripper = Gripper(arm)
if not (self._gripper.is_calibrated() or
self._gripper.calibrate() == True):
rospy.logerr("({0}_gripper) calibration failed.".format(
self._gripper.name))
raise
self._cuff.register_callback(self._close_action,
'{0}_button_upper'.format(arm))
self._cuff.register_callback(self._open_action,
'{0}_button_lower'.format(arm))
rospy.loginfo("{0} Cuff Control initialized...".format(
self._gripper.name))
except:
self._gripper = None
msg = ("{0} Gripper is not connected to the robot."
" Running cuff-light connection only.").format(arm.capitalize())
rospy.logwarn(msg)
def _open_action(self, value):
if value and self._gripper.is_ready():
rospy.logdebug("gripper open triggered")
self._gripper.open()
if self._lights:
self._set_lights('red', False)
self._set_lights('green', True)
def _close_action(self, value):
if value and self._gripper.is_ready():
rospy.logdebug("gripper close triggered")
self._gripper.close()
if self._lights:
self._set_lights('green', False)
self._set_lights('red', True)
def _light_action(self, value):
if value:
rospy.logdebug("cuff grasp triggered")
else:
rospy.logdebug("cuff release triggered")
if self._lights:
self._set_lights('red', False)
self._set_lights('green', False)
self._set_lights('blue', value)
def _set_lights(self, color, value):
self._lights.set_light_state('head_{0}_light'.format(color), on=bool(value))
self._lights.set_light_state('{0}_hand_{1}_light'.format(self._arm, color),
on=bool(value))
class RecordMotion(object):
def __init__(self, arm, name, start):
uri = "mongodb://*****:*****@sawyer-mongo.documents.azure.com:10255/?ssl=true"
client = MongoClient(uri)
self.db = client.SawyerDB
self.collection = self.db.Commands
self.commandName = name
self.commandNumber = start
rp = RobotParams()
self._lastJoin = {}
self.lastButtonPress = datetime.datetime.now()
self._rs = intera_interface.RobotEnable()
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self._navigator_io = intera_interface.Navigator()
head_display = intera_interface.HeadDisplay()
head_display.display_image(
"/home/microshak/Pictures/Sawyer_Navigator_Main.png", False, 1.0)
valid_limbs = rp.get_limb_names()
self._arm = rp.get_limb_names()[0]
self._limb = intera_interface.Limb(arm)
if start == None:
limb = intera_interface.Limb("right")
limb.move_to_neutral()
print(self._arm)
# inputs
self._cuff = Cuff(limb='right')
self._navigator = Navigator()
# connect callback fns to signals
self._lights = None
self._lights = Lights()
self._cuff.register_callback(self._light_action,
'{0}_cuff'.format('right'))
try:
self._gripper = Gripper(self._arm)
#if not (self._gripper.is_calibrated() or self._gripper.calibrate() == True):
# rospy.logerr("({0}_gripper) calibration failed.".format(self._gripper.name))
# raise
except:
self._gripper = None
msg = ("{0} Gripper is not connected to the robot."
" Running cuff-light connection only.").format(
arm.capitalize())
rospy.logwarn(msg)
def clean_shutdown(self):
print("\nExiting example...")
if not self._init_state:
print("Disabling robot...")
# self._rs.disable()
return True
def record(self):
self._cuff.register_callback(self._close_action,
'{0}_button_upper'.format(self._arm))
self._cuff.register_callback(self._open_action,
'{0}_button_lower'.format(self._arm))
print "Registering COntrols"
ok_id = self._navigator.register_callback(
self._record_spot, 'right_button_ok'
) # The circular button in the middle of the navigator.
circle_id = self._navigator.register_callback(
self._record_OK, 'right_button_circle'
) #The button above the OK button, typically with a 'Back' arrow symbol.
show_id = self._navigator.register_callback(
self._record_start, 'right_button_show'
) #The "Rethink Button", is above the OK button, next to back button and typically is labeled with the Rethink logo.
while not rospy.is_shutdown():
rospy.sleep(0.1)
self._navigator.deregister_callback(ok_id)
self._navigator.deregister_callback(circle_id)
self._navigator.deregister_callback(show_id)
def _record_start(self, value):
self.headLight("green")
def _record_OK(self, value):
self.headLight("red")
print "we cool"
def _record_spot(self, value):
time = (datetime.datetime.now() - self.lastButtonPress).seconds
print time
print "!!!!!!!!!!!!!"
if time < 2:
print "time to bail"
return
self.lastButtonPress = datetime.datetime.now()
print "spot record"
self.headLight("red")
self.commandNumber += 1
posts = self.db.Command
joints = {}
names = self._limb.joint_names()
for join in names:
joints.update({join: self._limb.joint_angle(join)})
if joints == self._lastJoin:
return 0
self._lastJoin = joints
post = {
"Name": self.commandName,
"Order": self.commandNumber,
"Action": "Move",
"Joints": joints
}
posts.insert(post)
print post
self.headLight("green")
def _open_action(self, value):
self.headLight("red")
if value and self._gripper.is_ready():
self._gripper.open()
self.commandNumber += 1
posts = self.db.Command
post = {
"Name": self.commandName,
"Order": self.commandNumber,
"Action": "Gripper",
"Open": True
}
posts.insert(post)
self.headLight("green")
def _close_action(self, value):
self.headLight("red")
if value and self._gripper.is_ready():
rospy.logdebug("gripper close triggered")
self._gripper.close()
self.commandNumber += 1
posts = self.db.Command
post = {
"Name": self.commandName,
"Order": self.commandNumber,
"Action": "Gripper",
"Open": False
}
posts.insert(post)
self.headLight("green")
def _light_action(self, value):
if value:
rospy.logdebug("cuff grasp triggered")
else:
rospy.logdebug("cuff release triggered")
if self._lights:
self._set_lights('red', False)
self._set_lights('green', False)
self._set_lights('blue', value)
def _set_lights(self, color, value):
self._lights.set_light_state('head_{0}_light'.format(color),
on=bool(value))
self._lights.set_light_state('{0}_hand_{1}_light'.format(
self._arm, color),
on=bool(value))
def headLight(self, value):
colors = ["red", "blue", "green"]
for color in colors:
self._lights.set_light_state('head_{0}_light'.format(color),
on=bool(value == color))
def finalize(self):
print "ENDING!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
class GripperConnect(object):
def __init__(self, arm):
self._arm = arm
# inputs
self._cuff = Cuff(limb=arm)
self._gripper_position = []
self._max_gripper_position = 0.01
self._toogle = -1
self._recorder = []
self._data_number = 1
self._path = '/home/cloud/code/learn-dmp/data_collector/'
try:
self._gripper = Gripper(arm)
if not (self._gripper.is_calibrated()
or self._gripper.calibrate() == True):
rospy.logerr("({0}_gripper) calibration failed.".format(
self._gripper.name))
raise
self._cuff.register_callback(self._record_action,
'{0}_button_upper'.format(arm))
self._cuff.register_callback(self._open_action,
'{0}_button_lower'.format(arm))
rospy.loginfo("{0} Cuff Control initialized...".format(
self._gripper.name))
except:
self._gripper = None
msg = ("{0} Gripper is not connected to the robot").format(
arm.capitalize())
rospy.logwarn(msg)
def _open_action(self, value):
self._position = self._gripper.get_position()
if value and self._gripper.is_ready():
if self._position > self._max_gripper_position:
rospy.logdebug("gripper open triggered")
self._gripper.close()
print 'gripper is closed'
print self._position
else:
rospy.logdebug("gripper close triggered")
self._gripper.open()
print 'gripper is opened'
print self._position
def _record_action(self, value):
if value and self._gripper.is_ready():
self._toogle = (-1) * self._toogle
if self._toogle == 1:
print 'Opening a new file named demo_' + str(self._data_number)
self._recorder = subprocess.Popen([
'rosrun', 'intera_examples', 'joint_recorder.py', '-f',
self._path + 'demo' + str(self._data_number) + '.txt'
],
stdout=subprocess.PIPE)
if self._toogle == -1:
print self._toogle
print 'Close the file'
self._data_number = self._data_number + 1
self._recorder.kill()